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Abstract  of  Dissertation  Presented  to  the  Graduate  School 
of  the  University  of  Florida  in  Partial  Fulfillment  of  the 
Requirements  for  the  Degree  of  Doctor  of  Philosophy 

PLANNING  COLLISION-FREE  PATHS  WITH 
APPLICATIONS  TO  ROBOT  MANIPULATORS 

By 

IHN  NAMGUNG 
DECEMBER  1989 

Chairman:  Dr.  Joseph  Duffy 

Major  Department : Mechanical  Engineering 

A new  approach  for  planning  a feasible  collision-free  path  based 

on  a Bezier  curve  of  order  two,  a parabola,  has  been  developed  for  two  and 

three  dimensional  environments.  It  is  assumed  that  obstacles  are 

stationary  and  the  shape  and  location  of  obstacles  are  known.  The 

algorithm  involves  geometric  analysis  and  produces  a path  connecting  a 

user  specified  start  and  target  point  which  does  not  interfere  with  any 

obstacles.  The  algorithm  consists  of  three  parts:  a geometric  mapping  of 

obstacles,  selection  of  intermediate  points,  and  smoothing  of  the  final 

path. 

A geometric  mapping  transforms  obstacles  in  Euclidean  space  into 
obstacle  images  in  the  parameter  space  (6,  t) . The  main  property  of  the 
geometric  mapping  is  that  a point  in  the  parameter  space  which  is  not 
mapped  by  obstacle  images,  corresponds  to  a collision-free  parabola  in 
Euclidean  space.  If  the  entire  parameter  space  is  occupied  by  obstacle 
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images,  intermediate  connection  points  are  searched.  Selections  of 
intermediate  connection  points  are  made  near  the  vertices  of  the  obstacle 
that  blocks  the  straight  line  connecting  the  start  and  target  point. 
Geometric  mapping  is  then  carried  out  for  each  subinterval,  and  smoothing 
the  slope  discontinuity  at  each  connection  point  is  performed  to  ensure 
the  overall  smoothness  of  the  curve. 

Three  supporting  algorithms --expans ion  of  obstacles,  collision 
recognition,  and  change  of  configuration  without  changing  the  end-effector 
position- -are  developed  for  collision  avoidance  of  a planar  3R  robot  and 
a General  Electric  P60  robot.  Expansion  of  obstacles  is  introduced  to 
accommodate  the  size  of  the  end-effector.  Collision  recognition  of  robot 
links  is  carried  out  by  checking  interferences  among  robot  links  and 
obstacles.  Change  of  configuration  by  rotating  orientation  of  the  end- 
effector,  without  altering  the  position,  is  introduced  to  alleviate  the 
problems  associated  with  an  attempt  to  move  out  of  the  reachable 
workspace,  exceed  the  valid  range  of  joint-actuators  or  collide  with 
obstacles . 

Animated  simulation  of  collision-free  path  generation  and  collision 
avoidance  of  a planar  3R  robot  and  a General  Electric  P60  robot  is 
implemented  on  a Silicon  Graphics  4D/70  graphics  workstation. 
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CHAPTER  1 
INTRODUCTION 


The  key  characteristic  of  a robot  system  is  that  it  can  be 
programmed  to  perform  many  different  tasks . Robot  programming  can  be 
divided  into  three  levels:  task-level  control,  environment  coordination, 
and  manipulator  control.  These  levels  are  extensions  of  those  described 
by  Lozano-Perez  (1982).  Task-level  control  is  the  highest  level  motion 
specification.  For  example,  a user  specifies  that  a pin  should  be  placed 
in  a hole  rather  than  specifying  the  sequence  of  manipulator  motions 
needed  to  perform  the  insertion.  World  modelling,  path  planning,  grasp 
planning,  collision  avoidance  and  sensory  feedback  comprise  the 
environment  coordination  level.  Manipulator  control  is  directly 
responsible  for  controlling  the  robot;  manipulator  kinematics,  dynamics 
and  joint  actuator  control  belong  to  this  level.  Successful  task-level 
control  requires  a robust  environment  coordination  scheme,  thus  the  wide- 
spread use  of  industrial  robots  has  been  hampered  by  the  insufficient 
implementation  of  this  intermediate  level  of  control.  Path  planning  and 
collision  avoidance  are  studied  here  as  a step  toward  for  overcoming  these 
inadequacies . 

In  this  dissertation,  algorithms  are  developed  for  real-time 
planning  of  a feasible  collision-free  path  in  two  and  three  dimensional 
environments.  It  is  assumed  that  all  obstacles  are  stationary  and  fully 
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known.  Collision-free  path  here  refers  to  a series  of  points  joining  two 
designated  points  without  any  interfering  obstacles.  The  algorithms  for 
planning  collision-free  paths  are  based  on  planar  Bdzier  curves  of  order 
two,  parabola,  and  geometric  analysis.  Smoothness  of  the  path, 
specifically  continuity  in  the  first  derivative,  is  considered  in  the 
process  of  path  planning.  The  motivation  for  using  a Bezier  curve  of 
order  two  is  the  existance  of  a closed  form  solution  for  the  geometric 
mapping.  Closed  form  solutions  are  not  available  from  higher  order 
curves.  The  geometric  mapping  transforms  obstacles  in  Euclidean  space  to 
parameter  space;  unoccupied  points  in  the  parameter  space  ( 9 ,t ) correspond 
to  collision-free  paths  in  Euclidean  space. 

In  general,  there  are  two  types  of  problems  in  collision  avoidance. 
In  the  first  a robot  is  guided  along  a user  specified  path  while  avoiding 
collisions  with  obstacles  in  which  the  path  is  generated  separately.  In 
the  other  the  start  and  target  point  are  given  and  a robot  maneuvers  to 
the  target  point  while  avoiding  collisions  with  obstacles  in  which  the 
path  is  found  in  the  joint  space  of  the  robot.  The  first  case  can  be 
found  in  Young  and  Duffy  (1987)  and  it  is  computationally  effective.  The 
second  case  can  be  found  in  Brooks  (1983b)  and  it  appears  that  the  method 
is  computationally  prohibitive  for  robots  having  more  than  three  joints. 
The  first  case  is  considered  here  and  a path  is  provided  by  the  above  path 
planning  algorithms.  Algorithms  are  developed  for  expanding  obstacles  to 
accommodate  the  size  of  the  end-effector  and  links  as  part  of  path 
planning  and  collision  detection  respectively.  Other  algorithms  are 
developed  for  detecting  collisions  for  a planar  3R  robot  and  a General 
Electric  P60  robot.  Algorithms  are  also  developed  for  avoiding  the  limit 
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cases  such  as  collision  detection,  joint  limits  or  reachable  workspace 
limits  of  a planar  3R  robot  and  a General  Electric  P60  robot  by  changing 
the  configuration  without  altering  the  end-effector  position.  Change  of 
configuration  by  changing  both  orientation  and  position  of  the  end- 
effector  is  not  considered  here. 

A comprehensive  survey  of  path  planning,  collision  avoidance  for 
robot  manipulators  and  mobile  robots  is  presented  in  Chapter  2.  The 
review  of  mobile  robots  is  included  because  many  of  the  path  planning  and 
collision  avoidance  schemes  have  been  investigated  for  implementation  with 
mobile  robots.  These  algorithms  for  planning  collision-free  paths  include 
the  visibility  graph  search,  free  space  search,  potential  field  method, 
bug's  algorithm  and  others.  In  the  visibility  graph  search,  connections 
are  made  among  vertices  of  obstacles  and  start  and  target  points.  The 
connections  that  do  not  intersect  any  obstacle  are  organized  in  a tree 
structure.  The  shortest  path  is  found  by  search  methods  established  from 
AI  researches.  In  the  free  space  search,  the  workspace  is  divided  into 
several  cells  and  obstacle  occupancy  of  the  cells  is  determined  as  empty, 
mixed  and  full.  Mixed  cells  are  further  divided  into  smaller  cells  and 
occupancy  is  determined  again.  Empty  cells  are  organized  in  a tree 
structure  and  a connection  of  adjacent  empty  cells  from  start  to  target 
point  results  in  a path.  In  the  potential  field  method,  obstacles  are 
represented  by  repulsive  poles  and  the  target  point  is  represented  by  an 
attractive  pole.  A collision-free  path  from  start  to  target  point  is 
obtained  by  optimization  methods.  A method  resembling  a bug's  movement 
connects  start  and  target  points  by  straight  line  segments  and  boundaries 
of  obstacles.  A bug  follows  the  straight  path  toward  the  target  point. 
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If  an  obstacle  blocks  the  path  then  it  follows  the  boundary  of  the 
obstacle  until  it  returns  to  the  straight  path.  Other  schemes  include  a 
method  utilizing  Snell's  law.  Obstacle  avoidance  of  robot  manipulators 
is  divided  into  two  classes  of  problems.  One  assumes  that  a path  is 
given,  and  that  the  robot  maneuvers  to  avoid  collision  while  moving  along 
the  path.  The  other  assumes  that  the  path  is  generated  in  joint  space; 
as  a result  no  obstacle  avoidance  is  necessary.  Some  of  the  notable 
simulations  on  collision  avoidance  include  the  work  by  Udupa  (1977), 
Brooks  (1983b),  Maciejewski  and  Klein  (1985)  and  Young  and  Duffy  (1987). 
Collision  avoidance  of  a mobile  robot  may  be  attained  simply  by  expanding 
obstacles  and  planning  collision-free  paths. 

Conceptual  development  of  collision-free  paths  is  presented  in 
Chapter  3.  Fundamentals  of  parametric  curves  essential  to  this 
development  are  included  in  section  3.1.  The  geometric  mapping,  based  on 
Bezier  curves,  which  transforms  obstacles  in  Euclidean  space  to  the 
parameter  space  is  introduced  in  sections  3.2  and  3.3.  An  example  of  the 
geometric  mapping  for  a two  dimensional  case  is  shown  in  Figures  1-1  and 
1-2.  The  control  point  associated  with  a B6zier  curve  is  defined  by  the 
two  independent  parameters  (0,  t).  A selection  of  ( 9 , t)  in  the  clear 
area  of  parameter  space  results  in  a collision-free  path. 

Detailed  analysis  for  the  algorithms  of  planning  collision-free 
paths  is  described  in  Chapter  4.  The  geometric  mapping  consists  of  the 
determination  of  parameter  range,  calculation  of  intersection  between 
parabolas  and  obstacles  and  selection  of  a point  from  the  clear  area  of 
parameter  space.  When  the  entire  parameter  space  is  occupied  by  images 
of  obstacles,  a collision-free  parabola  is  not  available  for  the  given 
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Fig.  1-2  Obstacle  images  in  parameter  space 
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interval.  Intermediate  connection  points  are  searched  near  the  vertices 
of  the  obstacles  which  intersect  the  straight  line  connecting  the  start 
and  target  points.  After  finding  intermediate  connection  points,  the 
geometric  mapping  for  each  subinterval  determined  above  is  carried  out  and 
the  slope  discontinuity  at  the  connection  points  is  then  smoothed  with  a 
higher  order  curve.  In  the  case  of  three  dimensional  geometric  mapping, 
the  parameter  space  is  defined  by  three  parameters  (</> , 6,  t) , and  the 
parameter  <f>  is  assigned  first  to  define  the  projection  plane.  The  images 
on  the  projection  plane  are  obtained  by  intersections  between  obstacles 
and  the  projection  plane.  Then  the  geometric  mapping  developed  for  two 
dimensional  cases  is  applied  to  the  images  on  the  projection  plane.  If 
a projection  plane  does  not  produce  a collision-free  path,  the  parameter 
^ is  changed  and  the  above  processes  are  carried  out  again  until  a 
successful  path  is  found.  Some  of  the  results  of  simulation  for  the  two 
dimensional  collision-free  paths  are  given  in  Appendix  A. 

In  Chapter  5,  collision  detection  and  change  of  configuration  of 
a robot  manipulator  are  developed  as  part  of  collision  avoidance  of  a 
planar  3R  robot  and  a General  Electric  P60  robot.  As  mentioned 
previously,  collision  avoidance  considered  in  Chapter  5 is  that  a robot 
is  guided  along  a given  path  while  avoiding  collisions.  It  is  assumed 
that  the  orientation  of  the  end-effector  is  free  to  change  and  the 
position  cannot  be  altered  during  the  change  of  configuration.  Collisions 
among  robot  links  and  obstacles  are  identified  by  checking  intersection 
of  links  with  obstacles.  Robot  links  are  represented  by  line-segments  to 
reduce  necessary  computation  for  interference,  and  obstacles  are  expanded 
to  accommodate  the  size  of  link.  As  the  robot  moves  along  the  path  with 
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its  initial  orientation,  the  limit  cases,  described  previously,  are 
monitored.  Should  such  an  event  occur,  configuration  of  the  robot  is 
changed  in  real-time  while  staying  at  the  same  position  by  simply  rotating 
the  orientation  of  the  end-effector.  For  a General  Electric  P60  robot, 
several  rotation  axes  are  pre-selected  and  the  actual  rotation  occurs 
about  the  axis  that  provides  the  most  significant  change  in  orientation. 
Algorithms  for  collision  detection  can  be  augmented  in  existing  robot 
trajectory  generators,  thereby  reducing  the  robot  programming  time. 

Animated  simulations  of  collision-free  path  planning,  and  collision 
detection  and  change  of  configuration  for  a planar  3R  robot  and  a General 
Electric  P60  robot  are  presented  on  a Silicon  Graphics  IRIS  4D/70  graphics 
workstation  using  the  C programming  language.  Some  of  the  results  are 


given  in  Chapter  5. 


CHAPTER  2 

SURVEY  OF  LITERATURE 


The  survey  is  focused  on  path  planning,  obstacle  avoidance  and 
mobile  robots.  Mobile  robot  strategies  resemble  obstacle  avoidance  for 
a point.  It  is  an  attempt  to  describe  the  current  state  of  research  in 
these  fields. 


2 . 1 Path  Planning  and  Collision  Avoidance 
The  algorithms  for  robot  path  planning  and  collision  avoidance  can 
be  grouped  into  the  following  categories: 

1.  Visibility  graph  search, 

2.  Free  space  search, 

3.  Potential  field  method, 

4.  Bug's  algorithm, 

5 . Others . 

The  work  by  Udupa  (1977)  is  regarded  as  one  of  the  first  for 
collision  detection  and  avoidance  of  a manipulator.  He  used  the  Scheinman 
arm,  which  has  a prismatic  joint  for  the  third  link.  Collision-free  path 
planning  was  carried  out  in  a two  level  hierarchy:  path  planning  for  the 
boom,  which  is  the  third  link,  was  done  first,  then  obstacle  avoidance  for 
the  wrist  joint  was  done.  Path  planning  for  the  boom  was  regarded  as  a 
planar  case  in  which  free  space  was  characterized  by  circular  sectors. 
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Path  planning  was  done  successively  from  start  position  to  goal  position 
through  free  circular  sectors. 

Visibility  graph  search  method  is  one  of  the  early  approaches  for 
planning  collision-free  paths.  Lozano-P6rez  and  Wesley  (1979)  introduced 
the  idea  of  visibility  graph  search.  Visibility  graph  (VGRAPH)  is  a set 
of  all  connected  links  among  vertices  of  obstacle  images  in  configuration 
space,  including  start  and  goal  position,  that  do  not  intersect  with 
obstacles.  The  shortest  collision-free  path  from  start  to  goal  is  the 
shortest  in  the  VGRAPH  from  the  node  corresponding  to  start  to  that 
corresponding  to  goal  when  the  Euclidean  metric  is  used  on  the  links  in 
the  VGRAPH,  see  Fig.  1-1.  An  A*  graph  search  is  used  to  find  the  shortest 
path  from  the  VGRAPH.  A*  is  a classical  minimum  cost  graph  search 
algorithm,  whose  optimality  properties  are  well  known  as  shown  by  Nilsson 
(1980).  The  algorithm  requires  the  moving  object  to  be  a point.  If  the 
moving  object  is  not  a point,  a new  set  of  obstacles  must  be  computed. 
The  operation  of  computing  a new  obstacle  from  an  original  obstacle, 
called  a grown  obstacle,  depends  on  the  movement  of  the  object.  If 
translation  is  allowed  and  rotation  is  not  allowed,  the  grown  obstacle 
operation  is  carried  out  by  expanding  edges  of  the  original  obstacles. 
Fig.  2-2. 

Configuration  space  of  an  object  is  defined  by  the  position  and 
rotation  of  the  body  in  Euclidean  space.  As  the  freedom  of  the  object 
increases,  the  VGRAPH  cannot  be  applied  in  a simple  manner.  Even  for  a 
planar  case  the  configuration  space  is  three  dimensional:  translation 
(x,y)  and  rotation  (0).  In  order  to  reduce  computing  time  the  rotational 
parameter  (0)  is  sliced  into  many  small  angles  and  the  VGRAPH  algorithm 
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Fig.  2-1  Visibility  graph,  (from  Lozano-P6rez  and  Wesley,  1979) 


Fig.  2-2  Grown  obstacles,  (from  Lozano-P6rez  and  Wesley,  1979) 
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is  applied  for  each  slice.  Since  the  VGRAPH  algorithm  does  not  take  into 
account  the  motion  of  the  robot  body,  it  is  applicable  only  for  the 
cartesian  robots  in  which  the  translational  degrees  of  the  robot  are 
separated  from  the  rotational.  Lozano-P6rez  (1981,  1983)  suggested  a swap 
volume  method  to  represent  the  rotational  links  and  the  gripper  of  the 
cartesian  robot  as  a bounding  box. 

A rigorous  mathematical  treatment  on  the  "Piano  Movers"  problem  was 
given  in  a series  of  papers  by  Schwartz  and  Sharir  (1983a,  1983b,  1983c, 
1984),  and  Sharir  and  Ariel-Sheffi  (1984).  The  problem  is: 

Given  two  configurations  (i.e.,  positions  and  orientations 
of  all  subparts)  of  the  bodies  B in  which  none  of  these 
bodies  touches  any  walls,  and  in  which  none  of  the  bodies 
B collide,  find  a continuous  wall-  and  collision-avoiding 
motion  of  all  the  B between  these  two  configurations,  or 
establish  that  no  such  motion  exists.  Schwartz  and  Sharir 
(1983b,  p.299). 

Moving  a two  dimensional  polygonal  body  amidst  polygonal  obstacles  was 
given  in  the  first  paper  of  the  series.  The  algorithm  is  polynomial  in 
the  number  of  walls  (CKn5)1  if  n is  the  number  of  walls).  In  the  second 
paper  the  case  of  arbitrary  number  of  moving  bodies,  some  of  which  may  be 
jointed,  was  treated.  It  was  shown  that  the  problem  can  be  solved  in  time 
that  is  polynomial  in  the  number  of  the  walls  and  the  bodies , but 
exponential  in  the  number  of  degrees  of  freedom  of  the  system  of  bodies. 
However,  even  for  a fixed  number  of  degrees  of  freedom,  the  algorithm 
presented  is  of  complexity  0(ne),  where  the  exponent  e can  be  high.  As 


1 0(n5)  indicates  that  the  number  of  geometric  operations,  such  as 
intersection  of  a point  with  a line-segment,  required  to  carry  out  the 
algorithm  is  at  most  as  large  as  n5. 
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a result,  this  general  algorithm  is  entirely  impractical  except  possibly 
for  the  simplest  cases. 

In  the  third  paper,  each  obstacle  was  enclosed  within  a circle,  and 
then  a path  is  planned  for  the  motion  of  these  circles.1  A more  precise 
algorithm,  taking  into  account  the  exact  geometry  of  the  moving  bodies, 
will  be  brought  to  bear  only  if  no  collision-free  motion  of  the  enclosing 
circle  exists.  The  problem  solved  in  the  fourth  paper  was  that  of 
planning  motions  for  a two  dimensional  robot  system  consisting  of  several 
arms  all  jointed  at  one  common  endpoint  and  free  to  rotate  past  each 
other.  The  algorithm  was  polynomial  in  the  number  of  wall  edges  0(nk+/*), 
where  k is  the  number  of  arms.  Planning  the  motion  of  a rod  moving  amidst 
polyhedral  objects  is  treated  in  the  fifth  paper. 

The  main  strategy  used  in  the  analysis  of  the  ’piano  movers' 
problem  was  an  investigation  of  free  configuration  space,  FP,  where  a body 
(or  group  of  related  bodies)  B is  constrained  to  move  freely  in  a two  or 
three  dimensional  space  bounded  by  certain  obstacles.  The  dimension  n of 
configuration  space  is  decomposed  into  two  subproblems  of  size  1 and  n-1, 
or  equivalently  projected  onto  a subspace  of  lower  dimension  n-1.  This 
decomposition  is  repeated  until  the  dimension  n is  reduced  to  one  or  two. 
A connectivity  graph  for  the  vertices  of  each  obstacle  is  formed  between 
the  projection.  The  advantage  of  this  technique  is  that  it  reduces  an  n- 
dimensional  problem  into  decomposition  problems  involving  manifolds  of 
lower  dimensions.  However,  each  step  of  projection  complicates 
significantly  the  geometric  structure  of  the  manifold  to  be  analyzed,  and 


1 


Such  simplification  can  also  be  found  in  Moravec(1980) . 
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so  recursive  application  of  the  technique  becomes  difficult  after  two  or 
three  levels  of  recursion. 

The  work  by  Bajaj  and  Kim  (1987,  1988)  is  concerned  with  effective 
construction  of  configuration  space.  In  their  first  paper  a planar  case 
with  translation  only  was  treated,  and  in  the  second  paper  configuration 
space  for  a sphere  was  treated  where  the  problem  was  reduced  to  planning 
motion  for  a point  among  grown  obstacles. 

Note  that  the  configuration  space  is  actually  the  same  as  joint 
space.  For  a 6R  industrial  manipulator  the  joint  space  is  6 - dimens ional , 
and  the  configuration  space  of  each  link  is  6 -dimens ional . However,  the 
constraint  imposed  on  each  joint  is  5 -dimens ional ; hence,  the 
configuration  space  for  the  manipulator  is  6x6-5x6  = 6.  One  of  the 
characteristics  of  the  free  configuration  space  which  contains  no 
obstacles  is  that  it  guarantees  collision  avoidance. 

One  of  the  most  successful  implementations  of  the  collision 
avoidance  algorithm  is  found  in  Brooks  and  Lozano-Perez  (1985)  where  a 
simulation  of  moving  an  object  through  a free  space  was  given.  Gouzenes 
(1984)  presented  an  implementation  of  planning  collision-free  path 
algorithms  based  on  Lozano-Perez ' s method.  An  implementation  by  Lozano- 
Perez  (1986,  1987)  is  motion  planning  for  simple  robot  manipulators,  where 
the  first  three  joints  were  treated  as  active.  Brooks  and  Lozano-Perez 
(1985)  reported  that  most  of  the  computing  time  was  spent  during  the  A* 
search. 

A simple  method  of  finding  collision-free  paths  was  introduced  by 
Brooks  (1983a).  Free  space  is  represented  as  overlapping  generalized 
cones.  Fig.  2-3.  Generalized  cones  are  constructed  by  examining  all  pairs 
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Fig.  2-3  Generalized  cones  generated  by  two  obstacles  and 
workspace  boundary,  (from  Brooks,  1983a) 

of  edges  of  obstacle  polygons.  Two  edges  are  accepted  as  defining  a 
candidate  generalized  cone  if  they  meet  the  two  requirements:  1)  at  least 
one  vertex  of  each  edge  should  be  on  the  outside  of  the  other,  2)  the  dot 
product  of  the  outward  pointing  normals  should  be  negative.  The 
generalized  cone  so  defined  may  not  lie  entirely  in  free  space;  there  may 
be  other  obstacles  which  intersect  it.  Each  obstacle  is  compared  to  the 
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generalized  cone.  If  the  intersection  occurs,  a modification  of  the 
generalized  cone  is  done.  The  center  line  of  a generalized  cone  is  a 
spine.  The  cones  are  examined  to  determine  if  and  where  their  spines 
intersect.  Each  spine  intersection  point  is  examined  for  orientations  of 
the  object  A in  which  it  is  guaranteed  to  be  completely  contained  in  the 
generalized  cone.  A connectivity  graph  is  built  based  on  spine 
intersection,  and  A*  search  is  used  to  find  a collision-free  path. 

In  Brooks  and  Lozano-P6rez  (1985),  the  configuration  space  is 
divided  into  rectangles.  Fig.  2-4.  Each  rectangloid  is  labeled  as  1) 
empty  if  the  rectangloid  does  not  intersect  a configuration  obstacle,  2) 
full  if  the  rectangloid  is  contained  within  a configuration  obstacle,  or 
3)  mixed  if  the  rectangloid  intersects  a configuration  obstacle  boundary. 
A free  path  is  searched  by  a connected  set  of  empty  rectangloid  cells  from 
starting  position  to  goal  position.  If  an  empty  cell  path  cannot  be  found 
in  the  initial  subdivision  of  configuration  space,  then  a path  that 
includes  mixed  cells  is  searched.  Mixed  cells  on  the  path  are  further 
subdivided  and  the  resulting  cells  are  labeled  as  empty,  full  or  mixed. 
Another  search  for  an  empty  cell  path  for  the  subportion  is  initiated. 
Gouzenes  (1984)  approximated  the  decomposition  of  the  configuration  space 
by  regular  grids.  A cell  is  empty  if  no  obstacles  are  inside  the  cell; 
otherwise  a cell  is  full.  All  the  empty  cells  are  represented  in  a tree 
structure.  A path  is  found  by  searching  through  the  tree.  Note  that  it 
is  not  necessary  to  carry  out  the  decomposition  in  the  configuration 
space . 

A grid  representation  of  workspace  was  given  by  Jun  and  Shin  (1988) 
and  Kambhampati  and  Davis  (1986).  In  Jun  and  Shin  (1988),  the  workspace 
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(a)  Configuration  space  without  rotation  of 
obstacles  cut  into  rectangular  cells 
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(b)  Connectivity  graph  for  cells  in  (a) 

Fig.  2-4  Configuration  space  represented  by  rectangular  cells  and 
associated  visibility  graph,  (from  Brooks  and  Lozano-Perez , 1985) 

was  represented  by  regular  cubes,  and  in  Kambhampati  and  Davis  (1986),  it 
was  represented  by  a quadtree.  A quadtree  is  a recursive  decomposition 
of  a two  dimensional  workspace  into  uniformly  sized  2'x2'  blocks.  Fig.  2- 
5,  where  i is  the  depth  of  a quadtree.  A node  of  a quadtree  represents 
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Fig.  2-5  Quadtree  representation,  (from  Kambhampati  and  Davis,  1986) 

a 2Jx2J  square  region.  A free  node  of  a quadtree  is  a node  of  the  quadtree 
representing  a region  of  free  space.  An  obstacle  node  is  a node 
representing  a region  of  obstacle.  A gray  node  is  a node  representing  a 
region  having  a mixture  of  free  space  and  obstacles.  A leaf  node  of  a 
quadtree  is  a tip  node  of  the  tree.  A collision-free  path  is  a connection 


of  free  nodes. 
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Grid  based  path  planning  can  also  be  found  in  Mitchell  and  Keirsey 
(1984).  The  possible  connections  between  nodes  are  diagonal,  horizontal 
and  vertical.  The  search  for  a collision-free  path  is,  therefore,  much 
simplified.  Singh  and  Wagh  (1987)  and  Wu  et  al.  (1987)  also  used  a grid 
map  to  represent  the  workspace.  The  grid  map  depends  on  the  size  of 
obstacles,  and  obstacle  edges  are  assumed  to  be  aligned  with  coordinate 
axes.  The  work  by  Elfes  and  Matthies  (1987)  is  sonar  and  stereo  sensor 
integration.  The  workspace  is  represented  by  an  occupancy  grid  map  using 
sonar  and  stereo  range  sensors.  Each  sensor  produces  a occupancy  grid  map 
separately.  The  final  global  occupancy  map  is  an  integration  of  both 
maps . 

Obstacle  avoidance  using  potential  fields  was  introduced  by  Khatib 

(1986).  The  key  idea  of  the  potential  fields  was  described  as: 

The  manipulator  moves  in  a field  of  forces.  The  position 
to  be  reached  is  an  attractive  pole  for  the  end  effector 
and  obstacles  are  repulsive  surfaces  for  the  manipulator 
parts.  Khatib  (1986,  p.91) 

Any  local  minimum  can  cause  the  manipulator  to  stop  at  an  unintended 
location  where  the  local  minimum  exists.  Volpe  and  Khosla  (1987) 
presented  superquadric  artificial  potentials  to  eliminate  local  minima. 
An  obstacle  is  expressed  in  terms  of  elliptical  isopotential,  Fig.  2-6. 
The  potential  field  method  is  conceptually  simple;  however,  it  has  local 
minima  that  cannot  be  eliminated.  It  eliminates  local  minima  only  if  one 
obstacle  is  associated  with  attractive  goal  potential;  however,  it  still 
has  local  minima  when  many  obstacles  are  cluttered  in  the  workspace. 
Newman  and  Hogan  (1987)  also  showed  collision  avoidance  of  moving 
obstacles  via  protective  potential  fields. 
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Fig.  2-6  A repulsive  potential  (for  an  obstacle)  and  an  attractive  well 
(for  the  target  point),  (from  Volpe  and  Khosla,  1987) 

Lumelsky  (1985,  1987)  and  Lumelsky  and  Stepanov  (1986)  presented 
a path  planning  method  resembling  a bug's  movement.  A point  follows  a 
straight  path  from  start  position  to  goal  position.  When  the  point  meets 
an  obstacle,  it  then  follows  the  left-side  boundary  of  the  obstacle  until 
it  returns  to  the  straight  path,  Fig.  2-7, a.  This  algorithm  does  not  find 
the  shortest  path  nor  the  safest  path,  and  may  not  guarantee  termination 
in  some  situation,  Fig.  2-7, b.  A modification  of  Lumelsky's  algorithm  is 
found  in  Dupont  and  Derby  (1988)  in  which  a collision-free  path  is  formed 
by  searching  from  start  position  and  goal  position  toward  the  mid  point 
until  they  meet.  The  search  strategy  is  the  same  as  Lumelsky's. 
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(a)  A path  is  found  (b)  The  method  does  not  guarantee  termination 


Fig.  2-7  A path  following  the  straight  path  and  the  boundary  of 
obstacles,  (from  Lumelsky  and  Stepanov,  1986) 

Obstacle  avoidance  as  presented  by  Gilbert  and  Johnson  (1985)  is 
an  algorithm  that  keeps  away  from  obstacles  by  testing  the  distance 
between  potentially  colliding  parts.  Richbourg  et  al.  (1987)  introduced 
yet  another  method  using  Snell's  law.  Snell's  law,  an  optic  law, 
characterizes  minimal  cost  paths  in  a routing  problem.  Given  start  and 
goal  positions,  the  shortest  path  is  found.  The  method  calls  for  a time 
consuming  optimization  process.  In  addition,  there  exist  blind  regions 
that  cannot  be  reached  by  any  path  obeying  Snell's  law. 


Obstacle  avoidance  of  a planar  3R  robot  was  shown  by  Young  and 


Duffy  (1987)  through  articulation.  The  path  was  predefined.  The 
allowable  range  of  rotation  of  each  link  was  computed  at  every  step  and 
the  configuration  of  the  robot  was  set  to  the  mid- value  of  the  allowable 
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range  of  rotation.  Any  obstacles  that  lie  within  the  circular  area  set 
by  the  link  reduce  the  allowable  range  of  rotation.  This  in  turn  changes 
the  configuration  of  the  robot  and  results  in  articulation  of  the  robot. 

Maciejewski  and  Klein  (1985)  and  Espiau  and  Boulic  (1986)  presented 
obstacle  avoidance  for  redundant  manipulators.  In  Maciejewski  and  Klein 
(1985),  the  method  is  to  augment  an  obstacle  avoidance  Jacobian  to  the  end 
effector  Jacobian  and  then  calculate  joint  angular  velocity  using  a 
pseudo- inverse . The  method  actually  relies  on  velocity  control.  In 
Espiau  and  Boulic  (1986),  proximity  sensors  are  used  to  detect  obstacles. 
The  proximity  information  is  incorporated  in  the  Jacobian  matrix  as  a 
repulsive  force.  The  work  presented  by  Hsu  et  al.  (1988)  was  focused  on 
the  control  of  a redundant  manipulator  rather  than  obstacle  avoidance . 
A dynamic  control  strategy  (joint  torque  control)  is  used  to  guarantee  the 
asymptotic  tracking  of  a desired  workspace  trajectory. 

Collision  avoidance  of  multiple  robots  is  another  active  research 
area  as  described  by  Freund  and  Hoyer  (1985,  1986,  1988),  Chien  et  al. 
(1988),  Nagata  et  al.  (1988)  and  Yeung  and  Bekey  (1987).  The  common  idea 
for  collision  avoidance  of  multiple  robots  is  to  set  a different  priority 
for  each  robot,  and  plan  collision-free  paths  for  each  robot  separately. 
Then  the  portions  of  paths  that  are  in  conflict  with  others  are  modified 
according  to  the  priority,  i.e.,  the  lower  priority  path  is  modified. 

2 . 2 Mobile  Robots 

Some  of  the  major  research  efforts  in  mobile  robots  include  the 
JPL  robot  by  Tompson  (1977),  Stanford  Cart  by  Moravec  (1980),  HILARE  by 
Giralt  et  al.  (1979,  1984),  IMP  (Intelligent  Mobile  Platform)  by  Crowly 
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(1985),  ALV  (Autonomous  Land  Vehicle)  by  Seida  and  Mogenthaler  (1987)  and 
HERMIES-IIB  (Hostile  Environment  Robotic  Machine  Intelligence  Experiment 
Series  II-B)  by  Burks  et  al.  (1987). 

The  JPL  robot  was  one  of  the  early  experimental  systems  that  was 
able  to  plan  a path  using  a visibility  graph  where  an  obstacle  was 
represented  by  algebraic  curves.  An  obstacle  was  impassable,  and  the 
visibility  graph  was  formed  by  connecting  the  edges  of  obstacles.  The 
terrain  model  was  provided  by  a vision  system. 

The  Stanford  Cart  was  also  able  to  plan  a path.  Obstacles  were 
represented  by  circles  and  the  shortest  path  was  found  from  the  visibility 
graph . 

HILARE  at  the  LAAS  laboratory  in  Toulouse,  France,  has  been  an 
experimental  system  since  1977.  The  path  planning  system  used  a 
connectivity  graph  which  was  a cellular  decomposition  of  free  space. 

IMP  requires  a global  model  having  a network  of  landmark  points. 
A path  planning  algorithm  provides  a global  path  from  the  land  marks.1 
A local  navigation  system  carries  out  the  planned  global  path,  maintains 
an  estimate  of  the  robot's  position  with  respect  to  the  global  model  and 
plans  local  paths  as  needed  to  avoid  unexpected  obstacles. 

ALV  of  Martin  Marietta,  Denver  Aerospace  Division,  was  capable  of 
navigating  a road.  ALV  was  equipped  with  a color  TV  camera  which  captures 
video  images  for  processing  by  the  vision  subsystem  and  a Laser  Range 
Imager  which  captures  an  image  of  range  values.  The  vision  system  locates 
road  edges  in  the  video  image  and  obstacles  in  the  range  image.  These 


A discussion  of  landmark  recognition  can  be  found  in  Nasr  and 
Bhanu(1988) . 


23 


images  were  used  to  generate  a reference  trajectory  and  for  obstacle 
avoidance.  An  obstacle  was  approximated  by  a bounding  box.  The  clearance 
between  the  road  edge  and  the  bounding  box  determined  which  side  the 
vehicle  would  pass . 

ALV  was  also  used  for  testing  a vision  system  developed  by  Daily 
et  al.  (1988)  and  Waxman  et  al.  (1987).  In  Daily  et  al.  (1988),  the 
actual  range  image  was  a rectangular  array  that  was  used  to  build  the 
Cartesian  Elevation  Map.  An  obstacle  was  represented  by  an  elevated  image 
on  the  grid.  A path  plan  was  carried  out  based  on  the  map;  a high  level 
reflexive  path  planner  set  a series  of  subgoals  which  described  the 
preferred  route  from  start  to  goal.  In  Waxman  et  al.  (1987),  the  scene 
analysis  for  navigation  of  ALV  was  focused.  Road  edges  were  represented 
by  connection  of  straight  line  segments  to  provide  long-,  intermediate-, 
and  short-range  navigation.  The  paths  generated  were  a "sequence  of 
regions"  (a  long-range),  a directional  heading  or  "corridor  of  free  space" 
connecting  the  current  region  with  the  next  region  (an  intermediate- 
range),  and  a "track  of  safe  passage"  through  the  selected  corridor  while 
avoiding  obstacles  (short-range). 

HERMIES-IIB  was  a sensor  based  mobile  robot,  developed  at  CESAR  of 
Oak  Ridge  National  Laboratory.  HERMIES-IIB  research  had  focused  on 
navigation  in  unknown  environments  and  visual  perception.  HERMIES-IIB  was 
equipped  with  an  IBM-7532  (an  industrial  version  of  the  IBM-AT)  with  2 
NCUBE  parallel  processing  boards  for  independent  control  and  scene 
analysis . 

The  control  system  for  a mobile  robot  requires  the  integration  of 
sensors,  modeling  of  the  world,  path  planning  and  task  execution.  Chatila 
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(1986)  proposed  a hierarchical  modular  decomposition  for  mobile  robot 
navigation.  Iyengar  et  al.  (1985)  proposed  a method  of  robot  navigation 
which  required  no  pre-leamed  model.  As  more  information  is  consolidated 
into  the  terrain  model,  the  building  polygons  of  the  obstacle  fit  more 
closely  and  the  polygons  representing  free  space  grow  larger.  Brooks 
(1896b,  1987)  proposed  a layered  control  system  for  a mobile  robot.  The 
control  system  was  decomposed  into  parallel  modules  based  on  task- 
achieving  behavior  which  is  contrary  to  the  conventional  ways  of  series 
decomposition  of  functional  units.  Ish-Shalom  (1987)  proposed  the  Funnel 
algorithm  for  task  level  robot  control.  The  idea  is  to  partition  the  task 
into  a sequence  of  local  sub-tasks  and  each  sub-task  is  carried  out  by 
local  feedback  law. 


2 . 3 Concluding  Remarks 

Most  of  the  works  related  to  collision  avoidance  and  path  planning 
are  still  in  beginning  stages.  Algorithms  used  are  either  explicit 
descriptions  of  free  space  or  the  potential  function  method.  Direct 
comparisons  between  algorithms  are  not  possible  because  no  standard 
problem  settings,  i.e.  the  size  of  workspace,  the  number,  shape  and  size 
of  obstacles,  have  been  proposed.  However  the  operations  involved  in 
these  previous  algorithms  are  known  to  be  computationally  expensive.  Some 
of  the  notable  simulations  are  those  by  Brooks  (1983b),  Brooks  and  Lozano- 
P6reze  (1985),  Espiau  and  Boulic  (1986),  Gouz6nes  (1984),  Kambhampati  and 
Davis  (1986),  Maciejewski  and  Klein  (1985),  Morevec  (1980)  and  Volpe  and 
Khosla  (1987)  and  Young  and  Duffy  (1987).  Hopcroft,  Schwartz  and  Sharir 
(1984)  suggested  a new  research  direction  to  the  problem: 
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. . . even  a certain  simple  two-dimensional  case  of  the 
coordinated  motion  planning  problem  for  multiple 
independent  objects  is  PSPACE-hard1 * . This  suggests  that 
the  problem  of  planning  coordinated  motions  of  many  bodies 
cannot  be  solved  efficiently  in  the  general  case,  a result 
that  should  direct  further  research  either  toward 
identifying  special  cases  of  the  problem  that  are  tractable 
or  toward  finding  more  practical  approximation  techniques. 
Hopcroft  et  al.  (1984,  p.87) 

A new  algorithm  presented  in  the  following  chapters  does  not 
involve  constructing  free  space  or  potential  fields.  The  algorithm 
directly  addresses  collision-free  paths  in  the  form  of  polynomial  curves. 


1 This  indicates  the  difficulty  of  the  problem  in  that  the  number 

of  geometric  operations  is  polynomial  such  as  n5  shown  in  p.ll. 


CHAPTER  3 

CONCEPTUAL  DEVELOPMENT  OF  COLLISION-FREE  PATHS 

In  this  chapter  fundamentals  of  parametric  curves  that  are 
essential  to  the  development  of  collision-free  paths  are  introduced.  Here 
a collision-free  path  refers  to  a series  of  points  connecting  two 
designated  points  without  interfering  any  obstacles.  A comprehensive 
treatment  on  parametric  curves  and  surfaces  can  be  found  in  a book  by  Faux 
and  Pratt  (1979) . 

After  the  treatment  of  parametric  curves,  fundamental  concepts 
essential  for  the  generation  of  collision-free  paths  such  as  geometric 
mapping,  selection  of  intermediate  connection  points  are  discussed.  A 
Geometric  mapping  transforms  obstacles  in  Euclidean  space  to  parameter 
space.  Images  of  obstacles  in  the  parameter  space  of  the  mapping  are 
deformed  and  may  overlap  each  other.  An  important  characteristic  of  a 
geometric  mapping  is  that  a point  from  the  clear  area  in  parameter  space 
corresponds  to  a collision-free  path  in  Euclidean  space. 

Intermediate  connection  points  are  required  when  the  parameter 
space  is  completely  occupied  by  images  of  obstacles.  Selection  of 
intermediate  points  is  made  near  the  vertices  of  obstacles  that  block  the 
straight  path  between  start  and  target  points.  The  geometric  mapping  for 
each  subinterval  is  then  carried  out  to  find  a collision-free  path  for  the 
subinterval . 
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A reduction  of  the  three  dimensional  collision-free  path  planning 
to  a two  dimensional  problem  reduces  the  algebraic  complexity  of  geometric 
mapping.  The  procedure  involves  projection  of  three  dimensional  obstacles 
onto  two  dimensions,  thereby  eliminating  one  variable.  A projection  plane 
is  formed  in  such  a way  that  it  contains  start  and  target  points;  thus, 
the  two  dimensional  geometric  mapping  can  be  applied  for  the  images  on  the 
projection  plane. 


3 . 1 Parametric  Curves 

There  are  several  ways  to  describe  curves,  namely  explicit  form, 
implicit  form,  and  parametric  form.  An  explicit  form  of  equation  is 
expressed  in  a form  such  as  Y = f(X)  for  the  two  dimensional  case.  An 
inverse  of  the  equation,  X = g(Y),  may  not  be  expressed  in  a closed  form, 
and  also  there  are  curves  that  cannot  be  described  in  explicit  form.  An 
implicit  form  of  equation  is  expressed  as,  for  example,  f(X,Y)  = 0 for  the 
two  dimensional  case,  which  has  similar  features  of  the  explicit  form. 

The  parametric  form  of  equations  have  been  studied  mainly  for 
applications  of  CAD/CAM  and  computer  graphics.  A parametric  curve  is 
expressed  in  the  form,  for  example,  X = Q(s),  Y = R(s)  for  the  two 
dimensional  case  where  s is  the  parameter.  Points  on  a parametric  curve 
can  be  obtained  sequentially  along  the  curve  by  varying  the  parameter. 
There  are  many  ways  of  representing  parametric  curves  including  spline  and 
Bezier  curves.  Each  representation  of  parametric  curves  has  special 
characteristics;  therefore,  it  is  important  to  choose  the  proper  one.  A 
Bezier  curve  of  order  two  is  used  throughout  this  dissertation.  This 
particular  curve  greatly  simplifies  the  geometric  mapping  in  which  the 
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mapping  of  obstacles  in  Euclidean  space  to  images  in  parameter  space  as 
a closed  form  solution  has  been  developed.  This  is  explained  in  sections 
3.2  and  3.3.  Other  curves  will  require  intense  computation  to  do  such 
mapping  by  iteration. 

3.1.1  The  Bezier  Curve 

Any  arbitrary  continuous  curve,  f(x),  of  finite  length  can  be 
approximated  to  any  desired  degree  of  accuracy  by  a polynomial,  p(x), 
which  is  known  as  the  Weierstrass  theorem,  Goult  et  al.  (1973).  A higher 
degree  of  accuracy  will  demand  a higher  degree  for  the  polynomial  p(x). 
The  polynomial  p(x)  can  be  represented  by  a combination  of  some  basis 

polynomial.  One  such  approximation  takes  the  following  form: 

n , n 

P(x)  = S kirn  i,N.  f(k/n)xk(l-x)n-k  = S f(k/n)B  (x)  . (3.1) 

k=0  k=0 

This  is  known  as  the  Bernstein  polynomial  approximation  of  degree  n.  The 
basis  polynomial  is  called  the  Bernstein  basis  for  the  set  of  all 
polynomials  of  degree  n and  is  expressed  as  follows: 

Bk(x)  = kf(n-k)!  xk<1-x>n'k  ««^1.0)  (3.2) 

where  k = 0,  1,  2,...,  n.  Since  the  basis  is  defined  in  the  interval 
0-x^l,  in  some  cases  it  is  necessary  to  transform  an  arbitrary  interval, 
a<r<b,  to  the  interval  0<x<l.  The  desired  finite  interval  a<r<b  can  be 
transformed  to  0<x<l  by  the  formula  r=(b-a)x+a.  This  linear  relationship 
between  x and  r does  not  distort  the  original  curve.  Topics  related  to 
polynomials  are  discussed  at  greater  length,  for  example,  in  Goult  et  al. 
(1973)  . 
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Bezier  (1972)  extended  the  idea  of  approximation  of  a function  to 
approximation  of  a polygon,  in  which  n+1  vertices  of  a polygon  are 
approximated  via  the  Bernstein  basis.  If  the  scalars  f(k/n),  a known 
function  value  at  x=k/n,  are  replaced  by  the  polygon  vertices 
consecutively  in  terms  of  X,Y,Z  coordinate  values,  the  resulting  vector- 
valued polynomial  represents  a Bezier  curve  segment  of  degree  n.  It  is 
also  called  a Bemstein-Bezier  polynomial  curve.  For  instance  the  X 
coordinate  of  the  Bezier  curve  is  expressed  as  follows; 


where  rkx  is  the  X coordinate  of  vertex  rk,  s is  a parameter  and  the  valid 
interval  is  0£s<l.  Y and  Z coordinates  are  expressed  in  a similar 
fashion.  The  parameter  x in  (3.1)  is  replaced  by  s and  it  should  not  be 
confused  with  the  X coordinate  of  the  vertices.  When  n=2,  the  basis 
consists  of  BQ(s)  = (l-s) 2 , B1  (s)=2s (1-s)  and  B2(s)=s2,  and  the  resulting 
curve  is  a parabola  and  it  approximates  the  triangle: 


The  equation  (3.4)  can  be  derived  geometrically,  see  Fig.  3-1. 
Points  rg,  rfa  and  ^ divide  line  segments  r0r1 , r.,r2  and  r_rb.  respectively, 
by  a ratio  s:(l-s).  The  parabolic  curve  is  a path  traced  out  by  point  r£, 
which  is  an  approximation  to  the  triangle  EqT^.  The  curve  can  be  derived 
as  follows; 


rkxsk(l-s)"-k 


(3.3) 


r = ^(l-s)2  + r,j2s(l-s)  + r2s2 
= (r0-2r1+r2)s2  + 2(r1-rQ)s  + r 0 


(3.4) 


-a  = <^1  - £o>s  + *0 

£b  = (r2  - r^s  + ry 


Ec  = (Eb  - £a>S  + —a 
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Substituting  rg  and  ^ in  the  right  side  yields 
Kc  = (ro'Z^+rjJsZ  + 2(r1-r0)s  + 

The  valid  interval  of  the  parameter  s is  0<x<l  where  s=0  corresponds  to 
point  5q  and  s=l  corresponds  to  point  r2.  The  process  can  be  generalized 
for  a polygon  of  n vertices  and  a Bezier  curve  of  order  n-1  results.  One 
should  note  that  in  Fig.  3-1  the  lines  r0£1  and  r^r2  are  tangents  to  the 
curve  at  points  rg  and  r2  respectively.  The  location  of  vertex  r1 
determines  the  shape  of  the  parabola.  The  parametric  parabola  derived 
above  has  special  meaning  in  developing  collision-free  paths  in  which  r^, 
r2  and  r1  are  viewed  as  start,  target  and  control  point,  respectively. 
3.1.2  Tangents  and  Normals  to  a Parametric  Curve 

Tangents  and  normals  are  important  properties  of  a curve.  Tangents 
and  normals  to  a curve  in  explicit  form  are  well  known  from  algebraic 
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geometry.  The  following  discussion  shows  the  relationship  between 
explicit  form  and  parametric  form.  A tangent  at  point  (x0,y0)  on  a curve 
y=f(x)  is  expressed  as  follows: 


y 


(x0-y0) 


(x-xQ) 


(3.5) 


It  is  possible  to  derive  a tangent  in  parametric  form  from  this  equation 
with  an  assumption  that  the  curve  has  a known  parametric  form  P(x(s),y(s)) 
with  s being  the  parameter.  The  equation  is  rearranged  in  terms  of  x and 
y using  the  chain  rule.  A substitution  of  s=sQ  for  the  point  (x0,y0) 
yields 


$ -S]s-So(,c-V  - u 


(3.6) 


where  u is  a new  parameter  along  the  tangent.  The  tangent  is 

x (u)  = X0  + U-jp  ] 

o ds  J s=s„ 


y(u)  = y0  + 


S — sn 


(3.7) 


(3.8) 


The  normal  of  the  curve  at  point  s=sQ  can  be  derived  in  a similar  fashion 
where  the  derivative  of  equation  (3.5)  is  replaced  by  -dx/dy: 


x 


y 


x0  + u 


y0  - u 


s=sr 


s=sr 


(3.9) 

(3.10) 


3.1.3  Coordinate  and  Parameter  Transformation 

Rotation  or  translation  of  the  Bezier  curve  can  be  achieved  by 
simply  transforming  the  vertices  that  define  the  curve.  A coordinate 
transformation  of  a parametric  parabola  is  carried  out  by  multiplying  by 
transformation  matrix  M.  The  equation  (3.4)  can  be  written  in  a matrix 
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form  in  which  r,  r^,  and  r2  are  column  vectors  in  homogeneous 

coordinates : 


[ Eo  Ei  r2  ] 


1 

" 

1-2  1 

1 

0 2-2 

s 

0 0 1 

s2 

- 

. 

R U S 


(3.11) 


Multiplying  by  the  transformation  matrix  yields 


M r 


E = M [ Eq  E,  E2  ] 


' 

r i 

1-2  1 

l 

0 2-2 

s 

0 0 1 

s2 

- 

_ 

MRUS 


(3.12) 


- R*  U S 

Equation  (3.12)  shows  that  a transformation  of  vertices,  R*=M  R,  is 
equivalent  to  the  transformation  of  parametric  curves. 

The  curve,  however,  may  be  constructed  by  a composite  of  different 
parametric  curves  each  having  different  intervals.  It  may  be  required  to 
make  each  curve  have  the  same  interval,  such  as  from  0 to  1 . A parameter 
transformation  takes  the  interval  sQ<s<s1  to  the  new  interval  0<s*<l.  The 
parameter  transformation  must  be  linear  in  order  to  preserve  the  degree 
of  the  polynomial.  It  is  expressed  in  the  following  form,  for  the  first 
order  term: 

s - (l-s*)sQ  + s*s1  = s0  + (srs0)s*  . (3.13) 

A higher  degree  of  s can  be  obtained  from  the  formula 
n 

sn  = S (J).Q»  ((s1-s0)s*)n‘i 
i=0 


(3.14) 
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which  is  a binomial  expansion.  For  instance,  the  column  vector 
representing  parameter  s in  equation  (3.12)  can  be  transformed  from 
interval  sQ<s<s1  to  0<s*<l  by  substitution  using  equations  (3.13)  and 
(3.14): 


0 

(srs0) 

2so(si-s0) 


0 

0 

(si “sn>  2 


s 

s*2 


= T S*  (3.15) 

where  T is  the  transformation  matrix  and  the  new  parameter  is  defined  in 
the  interval  0<s*<l.  T is  nonsingular  unless  sQ  and  s1  are  coincident. 
A parameter  transformation  can  also  be  represented  as  a transformation  of 
vertices : 

r = RUS  = RUTS*  . (3.16) 

A matrix  R'  representing  the  new  vertices  for  r may  be  given  by: 

R'  = R U T U'1  , (3.17) 

then 


R'  U = R U T . (3.18) 

Substituting  (3.18)  to  (3.16)  yields: 

r = R'U  S*  . (3.19) 

This  is  the  same  form  of  equation  as  (3.11),  and  the  matrix  U is  always 
nonsingular,  equation  (3.11).  The  matrices  T and  T*  become  identity 
matrices  if  the  valid  interval  of  s is  0<s<l. 

3.1.4  Smooth  connection  of  two  parametric  curves 

It  is  often  required  to  connect  two  curves  and  to  maintain 
continuity  of  the  first  derivative^1  continuity)  or  continuity  of  both 


34 


the  first  and  second  derivative(c2  continuity)  of  the  entire  curve.  In 
the  following  subsections,  four  different  methods  of  smooth  connection  in 
c1  continuity  are  described.  Curves  to  be  connected  are  denoted  by  C0  and 
C1  in  the  figures,  and  they  are  not  necessarily  touching  each  other.  The 
curve  indicated  by  C2  is  the  smoothing  curve . 


\ 

\ 


Fig.  3-2  Connection  by  using  a parabola 
Connection  by  a parabolic  curve 

Fig.  3-2  shows  a smooth  connection  via  a parabolic  curve,  a Bezier 
curve  of  order  two.  Pq  is  a point  on  curve  CQ  and  P1  is  a point  on  curve 
C1 . Tangents  at  points  Pq  and  P1  are  obtained  by  using  equations  (3.7) 
and  (3.8),  and  P2  is  the  intersection  point  of  the  tangents.  Pq,  P2  and 
P1  form  a triangle  and  a connecting  parabola  can  be  constructed  using  a 
Bezier  curve.  The  solid  line  is  the  resulting  smooth  curve.  This  method 
cannot  be  used  when  two  tangents  are  parallel  or  the  intersection  point 
is  out  of  bounds,  or  a sharp  edge  at  the  connection  point  occurs  as  in 
Fig.  3-3.  The  smoothing  process  alters  the  valid  interval  of  parameter 
of  curve  CQ  and  C1 . A parameter  transformation  may  be  needed  to  result 
in  the  same  interval  for  each  curve  CQ,  C1  and  C2. 
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Direct  combination  of  two  parametric  curves 

Fig.  3-4  shows  a combination  of  the  specified  interval  of  two 
curves.  The  combination  can  be  viewed  as  a blending  of  two  curves.  The 
combination  basis  function  can  be  devised  to  provide  a c1  or  c2  continuity 
at  connection  points.  In  this  subsection  a combination  basis  function 
that  gives  c1  continuity  is  discussed.  A linear  combination  of  two  curves 
cannot  be  used  since  it  does  not  provide  c1  continuity  at  the  connection 
point.  The  curves  CQ  and  C1  have  their  own  parameters  so  there  are  two 
different  parameters  associated  with  the  curves.  To  reduce  the  number  of 
independent  parameters,  the  valid  interval  of  each  parameter  is 
transformed  to  make  the  parameter  interval  the  same . This  allows  the  two 
different  parameters  to  be  treated  as  one  parameter.  The  curves  P[(s)  and 
Pn(t)  in  Fig.  3-4,  indicated  by  dotted  lines,  are  the  sections  to  be 
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Fig.  3-4  Combination  of  two  parametric  curves 
combined.  The  curve  C2  shows  the  combination  of  Pj(s)  and  Pn(t)  and  takes 
a form  given  by 


C2  = (1  - f(u))Pj(s)  + f (u)PM(t)  (3.20) 

where  f(u)  is  the  combination  basis  function  in  a parametric  form.  The 
range  of  parameter  u can  be  set  to  be  the  same  as  s and  t,  [0,1],  so  the 
parameters  s,  t and  u are  treated  as  one  parameter.  The  combination  basis 
function,  f(u),  has  to  satisfy  the  following  conditions  to  meet  c°  and  c1 
continuity.  A higher  order  continuity  can  be  obtained  by  imposing 
additional  conditions: 


C2(0)  = [PI(s)]s=Q 
C2'(0)  = [P,'(s)]s=0 

(3.21) 

C2(l)  = [P„(t)]t=1 
C2'(l)  = [P„'(t)]t=1 

These  conditions  require  the  combination  basis  function  to  be: 


f (0)  = 0 
f(l)  = 1 
f'(0)  = 0 


(3.22) 


f'(l)  = 0 
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Fig.  3-5  Combination  basis  function 

Equation  (3.22)  leads  to  a combination  basis  function  of  order  three,  a 
cubic  function: 

f (u)  = u2 (3-2u)  . (3.23) 

A combination  basis  function  based  on  equation  (3.23)  is  shown  in  Fig.  3-5. 
It  is  important  to  see  that  the  valid  interval  of  parameters  of  Pj(s), 
Pn(t)  and  f(u)  are  the  same,  so  they  can  be  treated  as  one  parameter. 
Equation  (3.20)  yields 

c2(v>  = (p„(v)  - p, (v))v2 (3-2v)  + P,(v)  (3.24) 

where  v is  the  new  parameter  representing  the  parameters  s,  t and  u.  The 
order  of  the  curve  C2(v)  is  increased  by  the  order  of  the  combination 
basis  function.  The  curve  C2(v)  is  confined  in  a region  surrounded  by  the 
curves  Pj(s)  and  Pn(t). 
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Connection  by  cubic  curve 

Instead  of  blending  sections  of  two  curves,  smoothing  can  be 
carried  out  using  a cubic  curve.  Since  four  coefficients  are  involved  in 
a cubic  function,  four  conditions  are  necessary  to  establish  the  unknown 
coefficients.  The  conditions  are  c^  and  c^  continuity  at  the  connection 
points  Pq  and  P1 . Fig.  3-6  shows  the  result  of  connection  where  C2(u)  is 
the  connecting  cubic  curve.  C2(u)  in  parametric  form  is  expressed  in 
general  by 


where  aQ,  a1 , bQ,  b1 , cQ,  c1 , dQ  and  d1  are  unknown  coefficients.  The  valid 
interval  of  parameter  u is  0<u<l.  The  conditions  are 


- x2(u)  = aQu3  + bQu2  + cQu  + dQ 


(3.25) 


C0Cs)]s=s*  = C2(u)]u=Q 
C0<S>']S=s*  = C2<u>']u=0 

Ci<t)]t=t*  = c2(u)]u=1 
C^t  )'lt=t*  = C2(u)']u=1 


(3.26) 


\ 


\ 


Fig.  3-6.  Connection  by  a cubic  curve 
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Substituting  equation  (3.26)  into  (3.25)  with  x and  y components  of  CQ(s) 
and  (t)  being  (xQ(s)  ,y0(s) ) and  (x^t)  .y^t))  respectively,  the  unknown 
coefficients  of  equation  (3.25)  are  obtained  as  follows: 


= x0(s*> 

(3.27) 

= y0<s*> 

(3.28) 

dxQ(s)  -I 

ds  -*  s=s* 

(3.29) 

dy0(s)  n 

- -3 — J * 

ds  J s=s 

(3.30) 

= -2(xl(t*)-cx-dx)  + 

dx1 (t)  i 

— 1 I *-  Cx 

dt  J t-t  x 

(3.31) 

= -2(y1 (t*) -cy-dy)  + 

dy,(t)  -i 

— : J *'  cy 

dt  J t-t  y 

(3.32) 

Fig.  3-7  Out  of  bound  connection  curve 
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3(x! (t  )-cx-dx)  - 


b1  = 3(y1(t*)-cy-dy)  - 


dx^t) 

dt 

dy^t) 

dt 


U- 

u 


+ c_ 


+ c. 


(3.33) 


(3.34) 


This  method  requires  calculation  of  derivatives.  There  is  a possibility 
that  the  connecting  cubic  curve  can  be  out  of  bounds,  as  shown  in  Fig.  3-7. 
These  erratic  properties  of  cubic  connection  curves  have  to  be  carefully 
considered  before  applying  the  method. 

Connection  by  a quart ic  curve 

If  an  additional  point  is  specified  for  the  connection  curve  to 
pass  through,  a quartic  curve  has  to  be  used  as  a connection  curve.  The 
additional  point  can  effectively  limit  the  connection  curve  in  certain 
regions.  If  a connection  curve  has  to  meet  the  requirements  of  c°  and  c1 
continuity  at  connection  points  as  well  as  to  pass  through  an  additional 
point,  a quartic  curve  connection  is  necessary  because  the  additional 
point  introduces  another  unknown  variable.  The  additional  point  also  can 
be  used  to  control  and  limit  the  shape  of  the  connection  curve.  Fig.  3-8 
shows  a quartic  curve  connection,  where  C2  is  a quartic  connection  curve 
and  P2  is  the  additional  point.  C2  in  parametric  form  is  expressed  as 
follows : 

- x2(u)  = aQu4  + bQu3  + cQu2  + dQu  + eQ 


C,(u) 


y2(u)  = a^4  + b^3  + c^2  + d.,u  + e1 


(3.35) 


where  aQ,  a1 , bQ,  b1 , cQ,  c1 , dQ,  d1 , eQ  and  e1  are  unknown  coefficients  and 
the  valid  interval  of  u is  0<u<l.  The  conditions  are  as  follows: 
C0(s)]s=s*  - C2(u)]u=0  - P^'V 
C0(s)']s=s*  = C2(u)']u=0  = V<P0x'.P0y') 
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Fig.  3-8  Connection  by  a quartic  curve 

c1<t)]t=t*  = C2(u)]u=1  = P^P^.P^)  (3.26) 

Whs  = c2<u)'i  a*1  = v<pix'-v> 

C2<u)]u=u*  = P2(P2x,P2y) 

The  coefficients  d0,  d1 , eQ  and  e1  are  easily  calculated  from  the  first 
two  conditions  of  equation  (3.35). 


d0  “ P0x 


e0  P0y 


d1  = V 


e1  = V 


(3.36) 


The  parameter  indicating  point  P1  is  u*  and  it  approximately  can  be  set 
by  the  ratio  of  PQP1  to 


u 


^2 


PqPi  + Pi?2 


(3.37) 


By  substituting  equation  (3.35)  into  (3.34),  one  can  get  the  following 
linear  equations  for  unknown  coefficients  aQ,  bQ  and  cQ: 


1 

■ 

■ 

111 

ao 

p - p - p ' 
r1x  r0x  r0x 

2 3 4 

bn 

SS 

*2  *3  *4 

u c u u 

0 

c° 

lx  Ox  * 

P2x  " P0x  ‘ P0x'u 

(3.38) 
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and  for  a1 , b1  and  c1 : 

' 

111 

2 3 4 

*2  *3  *4 

u c u u 

It  should  be  noted  that  the  parameter  indicating  P1 , u*,  is  in  fact 
arbitrary.  It  determines  the  overall  shape  of  the  connection  curve, 

therefore,  the  proper  value  is  important.  The  value  given  by  equation 
(3.37)  is  an  approximation  to  a parabolic  curve. 

3 . 2 Collision-Free  Paths  Connecting  Two  Designated  Points  in 
an  Obstacle  Strewn  Two  Dimensional  Environment 

In  this  section,  the  main  ideas  of  planning  collision-free  paths 
are  discussed.  A collision-free  path  refers  to  a series  of  points 
connecting  two  designated  points  which  does  not  interferes  with  any 
obstacles . 

In  this  problem,  the  start  and  target  points  are  given.  Further, 
all  obstacles  are  stationary,  eliminating  problems  associated  with  moving 
obstacles.  Obstacle  location,  size  and  shape  are  given  by  the  coordinates 
of  vertices. 

This  dissertation  considers  a collision-free  path  to  be  a 
polynomial  curve  connecting  two  designated  points  without  intersecting  any 
obstacles.  In  the  previous  chapter,  several  methods  of  planning 
collision-free  path  such  as  connectivity  graphs,  potential  function 
method,  free  space  search  method  and  Bug's  algorithm  are  reviewed.  Here 
for  the  first  time  a collision-free  path  generation  algorithm  employing 
a Bezier  curve  of  order  two  is  introduced  and  simulated.  The  motivation 
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for  use  of  a Bezier  curve  of  order  two  is  that  it  gives  control  of  the 
shape  of  the  curve,  and  permits  a closed  form  solution  for  the  geometric 
mapping.  The  geometric  mapping  transforms  obstacles  in  Euclidean  space 
to  parameter  space  (0,  t)  . The  main  property  of  geometric  mapping  is  that 
a point  in  areas  of  parameter  space  which  is  not  occupied  by  obstacle 
images  corresponds  to  a collision-free  path  in  Euclidean  space.  Hence  the 
geometric  mapping  provides  multiple  choice  of  paths.  Generalization  of 
the  geometric  mapping  based  on  a higher  order  Bezier  curve  is 
theoretically  possible,  however,  an  enormous  amount  of  computation  will 
be  required  to  carry  out  such  a geometric  mapping  as  the  solution  will  be 
approximated.  The  process  of  geometric  mapping  and  selection  of 
intermediate  connection  points  are  briefly  explained  later  in  this  chapter 
and  discussed  in  detail  in  Chapter  4. 

The  search  for  a collision-free  path  proceeds  follows:  a straight 
path  joining  the  start  and  target  point  is  examined.  If  the  straight  path 
is  suitable  then  the  problem  becomes  trivial.  However,  if  it  is  blocked 
by  an  obstacle,  a Bezier  curve  of  order  two  is  used  to  construct  a 
collision-free  path.  If  no  such  curve  exists  which  connects  the  start  and 
target  points  without  intersecting  any  obstacle,  a curve  with  higher 
degree  of  order  is  necessary  to  construct  a collision-free  path.  As  the 
geometric  mapping  of  such  higher  order  curves  is  not  feasible  for  a real- 
time solution,  the  path  is  partitioned  and  a recursive  application  of 
parabolic  curves  for  each  subinterval  of  the  start  and  target  point  is 
imployed.  Selection  of  intermediate  connection  points,  geometric  mapping 
of  subintervals  and  smoothing  the  slope  discontinuity  at  the  connection 
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points  are  then  carried  out.  An  overall  flow  chart  is  given  in  section 
4.5. 

For  extreme  cases  where  the  start  or  target  point  is  surrounded  by 
concave  obstacles  or  within  a maze  and  the  other  is  outside,  the  algorithm 
may  fail  to  find  a path.  This  issue  is  discussed  at  the  end  of  section 
3.2.2. 

3.2.1  Geometric  Mapping 

A B6zier  curve  of  order  two,  a parametric  parabola,  is  defined  by 
three  points  as  explained  in  Section  3.1.1.  Here  the  equation  (3.4)  is 
restated  in  x and  y components  asstiming  that  the  curve  is  in  the  x-y 
plane.  The  curve,  shown  in  Fig.  3.1,  is  defined  by 
x = (l-s)2r0x  + 2s(l-s)r1x  + s2r2x 

(3.40) 

y = (l-s)2r0y  + 2s(l-s)r1y  + s2r2y 

where  s is  the  parameter  and  rQ,  r1  and  r2  are  the  defining  vertices.  The 
end  points  rQ  and  r2  can  be  regarded  as  the  start  and  target  points, 

respectively.  The  shape  of  the  parabola  is  determined  by  r1  which  is 

hereafter  called  the  control  point.  The  control  point  can  be  arranged  in 
polar  coordinates  or  in  cartesian  coordinates.  In  the  following 

discussion  polar  coordinates  are  chosen  over  cartesian  coordinate  since 
better  shape  consistency  between  parameter  values  is  obtained.  By 

organizing  the  position  of  the  control  point  in  polar  coordinates  (0,t), 
intersections  between  parabolas  and  obstacles  can  be  plotted  in  parameter 
space  in  terms  of  the  value  of  (0,t). 

In  Fig.  3-9,  the  start  point,  S,  and  the  target  point,  T,  are 
equivalent  to  rQ  and  r2,  respectively,  in  equation  (3.40).  The  control 
point,  Q,  which  is  equivalent  to  r. , is  expressed  in  polar  coordinates 


Fig.  3-9  Obstacles  in  Euclidean  space 
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defined  by  the  two  parameters  9 and  t.  The  parameter  9 determines  a 
family  of  curves  and  the  parameter  t determines  the  length  of  the  curves. 
By  varying  9 and  t,  every  possible  parabola  that  is  inside  the  workspace 
and  passes  through  S and  T can  be  generated.  Thus  any  obstacle 
that  blocks  parabolas  creates  an  image  on  the  9- t space  as  shown  in  Fig. 
3-10.  The  geometric  mapping  is  defined  to  be  a transformation  of 
obstacles  in  Euclidean  space  to  parameter  space.  Dimensions  of  a 
parameter  space  are  determined  by  the  total  number  of  independent 
variables  associated  with  control  points.  For  instance,  in  Fig.  3-10,  9 
and  t are  the  independent  variables  associated  with  the  control  point  Q, 
hence  two  dimensional  parameter  space  results. 

When  a single  parabola  intersects  multiple  obstacles  their  images 
in  parameter  space  will  overlap.  Clear  areas  on  a parameter  space 
indicate  collision-free  parabolas.  Figures  3-9  and  3-10  show  this 


47 


relationship.  In  Fig.  3-9,  for  angle  9-6* , parabolas  defined  by  t in  the 
range  0<t<t1  intersect  obstacle  A and  those  with  t in  the  range  t^titj 
intersect  obstacle  B.  The  images  in  parameter  space  are  mapped  as  shown 
in  Fig.  3-10.  A point  having  the  smallest  value  of  t in  the  clear  area 
of  parameter  space  corresponds  to  the  shortest  parabolic  path  between  S 
and  T.  A path  that  may  be  regarded  as  the  safest  path  can  be  picked  from 
the  clear  area  by  finding  the  point  that  is  farthest  away  from  the 
surrounding  obstacle  images.  The  operations  involved  in  obtaining 
obstacle  images  and  selecting  a point  from  parameter  space  are  given  in 
Chapter  4. 

If  obstacles  are  cluttered  in  the  workspace,  the  parameter  space 
will  be  completely  filled  with  obstacle  images.  This  indicates  that  no 
collision-free  parabola  is  available.  One  solution  is  to  use  higher-order 
curves  such  as  Bdzier  curves  of  order  three.  The  main  problem  of  using 
higher  order  curves  is  that  the  curve  can  not  be  controlled  in  the  way  of 
a parabolic  curve.  For  Bezier  curves  of  order  three,  four  points  are 
involved  in  defining  the  curve  and  two  mid-points  can  be  regarded  as 
control  points.  A four-dimensional  parameter  space  results  because  two 
parameters  are  associated  with  each  control  point.  The  obstacle  images 
in  such  space  are  not  as  easily  obtained  as  in  two  dimensional  parameter 
space;  also  a higher  order  curve  may  contain  loops  in  the  designated 
interval.  In  the  following  section  a practical  alternate  solution  for 
these  problems  is  addressed. 

3.2.2  Recursion  of  Parametric  Parabolas 

As  discussed  in  the  previous  section,  a parameter  space  of 
dimension  greater  than  two  is  difficult  to  manage.  Instead  of  using 


48 


s 


T 


Fig.  3-11  Selection  of  an  intermediate  connection  point 
Bezier  curves  of  order  higher  than  two,  a piece-wise  connection  of 
parabolic  curves  can  be  used.  This  approach  reduces  the  original  problem 
to  a problem  of  determining  the  intermediate  connection  points.  The 
geometric  mapping  can  then  be  carried  out  for  each  sub  interval  to  find 
collision-free  path. 

A connection  point  cannot  be  selected  arbitrarily.  It  should  be 
within  the  workspace  and  outside  of  obstacles.  It  is  obvious  that  the 
obstacles  that  block  the  straight  path  should  be  circumvented.  These 
obstacles  can  be  identified  by  checking  the  intersection  with  the  straight 
path  ST.  Points  near  the  outer  edge  of  these  obstacles  can  be  selected 
as  intermediate  connection  points.  For  instance,  in  Fig.  3-11  vertices 
P1  and  P^  are  the  right-side  vertices,  and  P2  and  P3  are  the  left-side 
vertices  with  respect  to  the  line  segment  ST.  An  intermediate  connection 
point  can  be  selected  in  the  region  outside  of  obstacle  A.  It  is 
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Fig.  3-12  Smoothing  the  slope  discontinuity  of  two  connected  parabolas 
important  to  check  whether  the  selected  point  is  also  outside  of  other 
obstacles  to  verify  proper  selection. 

If  the  geometric  mapping  on  a subinterval  cannot  provide  a 
collision-free  path,  then  that  interval  is  further  divided  into  smaller 
sections.  In  other  words  the  process  of  finding  collision-free  paths  is 
carried  out  in  a recursive  fashion. 

A path  so  constructed  has  slope  discontinuity  at  the  connection 
points,  and  may  be  unsuitable  for  guiding  a robot,  see  Fig.  3-11.  Also 
the  connection  region  is  near  the  obstacle,  as  shown  in  Fig.  3-11,  hence 
the  path  may  be  unsafe  for  guiding  a robot . By  making  a smooth 
connection,  the  final  path  can  be  made  suitable  for  guiding  a robot 
further  away  from  any  obstacles,  which  makes  a safer  path,  as  shown  in 
Fig.  3-12.  There  are  several  ways  to  achieve  a smooth  connection  as 
described  in  Section  3.1.3.  Fig.  3-12  shows  a smooth  connection  of  two 
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adjacent  curves  using  the  method  of  direct  combination  of  two  parametric 
curves.  Since  the  smoothing  process  alters  the  original  path,  there  is 
a chance  that  the  connection  curve  may  interfere  with  obstacles.  The 
triangle  P2^l^2  Fig-  3-12  can  be  used  to  check  for  intersections  of  C.jC2 
with  an  obstacle.  Thus  the  smoothing  region  can  be  adjusted  to  avoid 
obstacle  interferences. 

For  some  extreme  cases,  such  as  either  the  start  or  target  point 
being  completely  surrounded  by  concave  obstacles,  or  either  point  inside 
a maze,  the  entire  parameter  space  is  then  occupied  by  obstacle  images. 
Shown  in  Fig.  3-13  is  a case  where  the  start  point  is  surrounded  by 
concave  obstacles  and  the  target  point  is  inside  a maze.  Obviously  the 
parabolas  connecting  the  points  S,  T will  interfere  with  obstacles,  and 


Fig.  3-13  Failure  of  planning  collision-free  path 
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Fig.  3-14  Parameter  space  for  Fig. 5-13 

the  entire  parameter  space  will  be  occupied  by  obstacle  images,  as  shown 
in  Fig.  3-14.  After  finding  intermediate  connection  points,  the  geometric 
mapping  for  each  subinterval  is  performed.  If  the  entire  parameter  space 
for  a subinterval  is  still  occupied  by  obstacle  images  like  Fig.  3-14, 
then  algorithm  will  fail  to  generate  a collision-free  path  for  such  cases. 
The  same  hatch  patterns  are  used  for  obstacles  in  Fig.  3-13  and  for  the 
images  of  corresponding  obstacles  in  Fig.  3-14. 

3 . 3 Collision-Free  Paths  Connecting  Two  Designated  Points  in 
an  Obstacle  Strewn  Three  Dimensional  Environment 

In  three  dimensions,  a control  point  can  be  expressed  in  spherical 

coordinates  (<f> , 6,  t)  as  an  extension  of  polar  coordinates  (0,  t)  of  two 

dimensions.  If  two  control  points  are  involved,  parameter  space  becomes 
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six  dimensional  and  geometric  mapping  is  practically  unfeasible.  Methods 
developed  for  the  two  dimensional  case  are  applicable  to  the  three 
dimensional  case.  Direct  manipulation  of  the  three  dimensional  problem 
is,  however,  algebraically  more  complicated.  This  can  be  avoided  by 
reducing  the  three  dimensional  problem  to  a two  dimensional  problem. 

The  following  discussions  illustrate  projections  of  three 
dimensional  objects  onto  a surface.  The  algorithm  for  two  dimensional 
collision-free  paths  can  then  be  used  without  modification.  Thus  planning 
collision-free  paths  in  three  dimensions  is  decomposed  into  two  steps: 
projection  of  obstacles  into  a plane  and  determination  of  valid  paths. 

Fig.  3-15  shows  a three  dimensional  obstacle  and  a Bezier  curve  of 
order  two  which  contains  one  control  point  denoted  by  P.  The  three 
parameters  0,  6 and  t determine  the  position  of  P.  The  parameter  0 merely 
determines  the  inclination  angle  of  the  flat  plane,  which  contains  the 
curve  itself,  from  the  reference  plane  set  by  start  point,  S,  target 
point,  T,  and  the  origin  of  the  workspace,  0.  The  reference  plane  can  be 
a vertical  plane  containing  the  points  S and  T.  Thus  the  geometric 
mapping  of  the  obstacle  images  on  the  flat  plane  can  be  obtained  for  the 
fixed  value  of  0 in  the  range  O<0<7r.  In  other  words,  the  flat  plane  is 
a projection  plane  and  obstacle  images  on  the  plane  can  be  treated  as  two 
dimensional  obstacles  so  the  geometric  mapping  developed  previously  can 
be  applied.  A three  dimensional  geometric  mapping  results  by  stacking  the 
two  dimensional  geometric  mapping  for  varying  values  of  0.  A collision- 
free  path  is  searched  on  the  projection  plane  for  varying  the  parameter 
values  from  0= 0°  to  0=180°.  After  varying  the  parameter  values  from  0= 0° 
to  0=180°,  the  search  may  not  find  a collision-free  path  if  all  of  the 
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Fig.  3-15  Intersection  of  an  obstacle  with  flat  projection  planes 


Fig.  3-16  Image  of  obstacle  on  the  projection  plane  defined  by  a 
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projection  planes  do  not  produces  a collision-free  path.  This  indicates 
a failure  of  finding  a collision-free  path.  Other  search  methods  for 
three  dimensional  case  are  not  attempted. 

Fig.  3-16  shows  the  image  of  the  obstacle  given  in  Fig.  3-15,  which 
is  the  intersection  of  the  obstacle  with  the  projection  plane.  The 
vertices  of  the  obstacle  image  are  the  intersection  points  of  edges  of  the 
obstacle  with  the  projection  plane.  The  projection  plane  is  defined  by 
three  points  S,  T,  and  P where  P determines  the  inclination  of  the  plane. 
The  plane  in  implicit  form  can  be  expressed  as  follows: 

Ax  + By  + Cz  + D = 0 (3.41) 

An  obstacle  edge  is  defined  by  two  end  vertices  and  can  be  described  in 
a parametric  form  such  as : 

x = <V1x-V0x>t  + V0x 

y - <VVt  + V0y  <3-42> 

z = <V1z-V0z)t  + V0z 

where  t is  the  parameter  and  the  valid  interval  of  t for  the  edge  is 
0<t<l.  Intersection  of  an  edge  of  the  obstacle  with  the  projection  plane 
can  be  determined  by  substituting  equation  (3.42)  into  (3.41).  If  the 
solution  for  t is  in  0<t<l,  intersection  occurs  and  the  intersection  point 
is  given  by  equation  (3.42).  Otherwise,  no  intersection  occurs. 
Intersection  points  so  calculated  have  to  be  organized  as  a closed  polygon 
as  in  Fig.  3-16.  Images  on  the  projection  plane  are  expressed  in  terms 
of  three  dimensional  coordinates.  It  is,  therefore,  necessary  to 
transform  them  to  two  dimensional  coordinates  in  order  that  the  algorithm 
developed  for  two  dimensional  collision-free  paths  can  be  used  without 
modification.  The  two  dimensional  collision-free  path  is  then  transformed 
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back  to  three  dimensions.  This  is  a coordinate  transformation  which  was 
explained  previously  in  Section  3.1.3. 

3 . 4 Concluding  Remarks 

This  chapter  has  described  the  essential  concepts  of  the  geometric 
mapping,  recursion  of  parabolas  and  reduction  of  three  dimensional 
problems  to  two  dimensional  problems.  The  geometric  mapping  transformed 
obstacles  in  Euclidean  space  to  a parameter  space  in  ( 6 , t)  in  which  a 
Bezier  curve  was  the  vehicle  for  the  mapping.  When  Bezier  curves  of  order 
two  cannot  provide  a collision-free  path,  recursion  of  parabola  is  used 
rather  than  geometric  mapping  based  on  higher  order  Bezier  curves  of  order 
higher  than  two.  Projection  onto  a flat  plane  was  introduced  to  reduce 
the  complexity  of  the  three  dimensional  problem. 

In  summary,  a collision-free  path  consists  of  parabolas.  Since 
curvature  of  a parabola  cannot  be  controlled  easily,  a collision-free  path 
may  be  unnecessarily  large.  This  problem  can  be  alleviated  by  finding  the 
shortest  path  which  corresponds  to  the  smallest  value  of  t in  parameter 
space  Figures  3-9  and  3-10. 

The  process  of  finding  collision-free  paths  breaks  down,  however, 
if  the  start  or  target  point  is  inside  of  a maze,  or  if  it  is  partially 
or  totally  enclosed  by  multiple  obstacles  or  by  concave  obstacles  as  shown 
in  Figures  3-13  and  3-14.  It  is  also  possible  that  an  intermediate 
connection  point  can  be  partially  enclosed  by  overlapping  obstacles  or 
concave  obstacles,  and  as  a result  the  process  may  not  proceed  further. 
In  implementing  the  algorithm,  such  events  can  be  monitored  by  noting  the 
depth  of  recursion. 


CHAPTER  4 

ANALYSIS  OF  COLLISION-FREE  PATHS 


In  this  chapter,  detailed  algorithms  for  planning  of  a collision- 
free  path  are  presented.  Recall  that  a collision-free  path  is  viewed  as 
a polynomial - curve , connecting  user  specified  start  and  target  points, 
without  intersecting  any  stationary  obstacles.  First  the  straight  paths 
connecting  start  and  target  point  for  two  and  three  dimensional  cases  are 
examined  before  carrying  out  the  geometric  mapping  to  determine  whether 
such  a path  is  feasible.  The  geometric  mapping  consists  of  the 
formulation  of  a parametric  parabola,  interference  of  the  parabola  with 
obstacles  and  selection  of  desired  collision-free  path  from  parameter 
space.  This  is  followed  by  recursion  of  parametric  paths  which  includes 
the  selection  of  intermediate  connection  points,  fundamental  mode  of  paths 
and  smoothing  the  sharp  connection.  In  three  dimensional  environments 
collision-free  paths  are  generated  by  first  projecting  three  dimensional 
obstacles  onto  two  dimensions  to  eliminate  one  variable  of  the  geometric 
mapping . 


4 . 1 Straight  Paths 

4.1.1  Straight  Path  in  Two  Dimensions 

Fig.  4-1  shows  a two  dimensional  robot  work  environment  where  start 

and  target  point  are  denoted  by  S=(S  ,S  ),  T=(T  , T ),  respectively,  and 

x y x y 


56 


57 


Fig.  4-1  Straight  path  and  two  dimensional  obstacles 


the  circular  obstacle  is  simplified  by  an  octagon.  The  straight  path  from 
start  to  target  point  is  represented  by  a parametric  equation,  G(u),  where 
u is  the  parameter,  and  indicated  by  a solid  line  segment  ST.  The 
validity  of  the  straight  path  is  examined  by  checking  for  intersections 
of  the  line  segment  ST  with  edges  of  obstacles.  Edges  of  obstacles  also 
are  represented  by  a parametric  equation,  for  instance  the  edge  P4P1  is 
given  by  H(v)  where  v is  the  parameter. 

r x(u)  = (Tx-Sx)u  + Sx 


G(u) 


(4.1) 


L y(u)  = (Ty-Sy)u  + Sy 


H(v) 


r x(v) 


<P1x-P4x>v  + P4x 


(4.2) 


y(v)  = (P1y-P4y)v  + P4y 

At  intersection,  G(u)  = H(v)  and  the  solutions  of  u and  v are  as  follows; 


(ySyXTx-Sx>  - (P4x-Sx><ySy> 

(P1x-P4x)<W  ' <W<VSx> 
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<P1y-y<Sx-P4x>  - (P1x-P4x)(W 

(pix-p4x)<VSv>  - <W<VSx> 


(4.4) 


When  the  solution  of  v from  (4.4)  is  0<v<l,  the  edge  of  the 
obstacle  does  intersect  with  the  line  ST,  however  it  does  not  necessarily 
intersect  with  the  line  segment  ST.  In  addition,  if  the  solution  of  u 
from  (4.3)  is  0<u<l,  the  edge  of  the  obstacle  intersects  with  the  line 
segment  ST.  If  the  denominator  of  (4.2)  vanishes,  edge  P^P.j  and  line  ST 
are  parallel  or  collinear;  such  special  cases  can  be  considered  as  non- 
intersecting. The  calculation  has  to  be  carried  out  for  all  edges  of  the 
obstacles  to  determine  the  interference  between  the  obstacles  and  line 
segment  ST.  If  all  edges  of  an  obstacle  do  not  intersect  with  line  ST, 
the  obstacle  is  located  on  one  side  of  line  ST.  The  location  of  the 
obstacles  can  be  determined  by  observing  the  sign  of  determinant.  A, 
formed  with  S,  T and  a vertex  of  the  obstacle: 


If  the  sign  of  the  determinant  is  positive,  the  obstacle  is  above  the  line 
ST,  otherwise  it  is  below  the  line  ST. 

4.1.2  Straight  Path  in  Three  Dimensions 

Interference  of  the  straight  path  with  obstacles  can  be  identified 
in  a way  similar  to  planar  case.  The  difference  is  that  intersections  are 
between  surfaces  of  obstacles  and  the  straight  path.  In  Fig.  4.2,  the 
line  L(u)  representing  line  segment  ST  is  given  by 
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(4.5) 


L(u)  = (T  - S)u  + S 


(4.6) 
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Fig.  4-2  Straight  path  and  three  dimensional  obstacle 

where  the  parameter  u is  defined  to  be  0<u<l  for  the  line  segment  ST. 
The  surface  A(s,t)  defined  by  vertices  VQ,  V1 , V2  and  V3  can  be  expressed 
in  a bilinear  parametric  equation,  where  s and  t are  parameters, 


A(s , t) 


Id-8)  S 


■ 

r i 

Vo  v3 

d-t) 

-1  —2 

t 

(4.7) 


where  the  parameter  range  of  s and  t are  0<s<l  and  0<t<l.  The  condition 
for  finding  the  intersection  point  of  the  surface  A(s,t)  and  the  line  L(u) 
is 

A(s,t)  = L(u)  . (4.8) 

After  substituting  equations  (4.6)  and  (4.7)  into  (4.8),  and  rearranging 
the  equation  yields: 
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Axst  + Bxs  + Cxt  + Dxu  + Ex  - 0 


AySt  + Bys  + Cyt  + Dyu  + Ey  = 0 


Azst  + Bzs  + Czt  + Dzu  + Ez  = 0 


(4.9) 


where  Ax  = VQx 


V1x  + V2x  * V3x 


-V0x  + V1x 


-V0x  + V3x 


D = S - T 

X X X 


Ex  = V0x 


Ay,  Az,  By,  Bz,  C Cz,  Dy,  Dz,  Ey  and  Ez  are  calculated  similarly  by 
substituting  y and  z components  of  S,  T,  Vq,  V1 , V2  and  V3.  Eliminating 
u from  equation  (4.9)  results  in  equations  in  s and  t. 


axst  + bxs  + cxt  + dx  = 0 


ayst  + bys  + cyt  + dy  = 0 


(4.10) 


where  a 


ax 

“ AxDz 

- AzDx 

ay  - Vz 

- AzDy 

b 

= B D 

- B D 

b = B D 

- B D 

X 

X z 

Z X 

y y z 

z y 

cx 

= CxDz 

- CzDx 

cy  “ CvDz 

- Cz°y 

dx 

- ExDz 

- EzDx 

dy  = EyDz 

- EzDy 

Again  s or  t can  be  eliminated  from  (4.10)  to  obtain  a quadratic  equation 
in  t or  s.  Elimination  of  t leads  to  a quadratic  equation  in  s: 

<ayVaxby>s2  + <bxVcxby+aydx-axdy>S  + (cydx-Cxdy>  = 0 * 

Since  there  is  only  one  intersection  between  a line  and  a plane  surface, 

the  discriminant  of  (4.11)  will  vanish  and  the  solution  of  s has  double 
roots.  The  double  roots  of  s are  given  by 


be  - cb  + ad  - ad 
xy  xy  yx  xy 

2 (a  b - a b ) 

' x y yx/ 


(4.12) 
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and  a corresponding  solution  for  t can  be  obtained  from  one  of  the 
equations  of  (4.10) 


V + dy 

V + cy 


(4.13) 


The  solution  for  u can  be  obtained  by  substituting  the  solutions  of  s and 
t into  one  of  the  equations  of  (4.9)  provided  that  the  denominator  does 
not  vanish.  If  the  solutions  for  s,  t and  u are  in  the  ranges  0<s<l, 
0<t<l  and  0<u<l  an  intersection  occurs,  since  0<s<l  and  0<t<l  represents 
the  surface  patch  defined  by  vertices  Vg,  V1 , V2  and  V3  of  the  obstacle 
and  0<u<l  represents  line  segment  ST. 

If  the  denominator  of  equation  (4.12)  or  (4.13)  vanishes,  the 
surface  and  the  line  are  parallel  or  coplanar  and  such  case  can  be 
considered  as  non- intersecting . The  calculation  has  to  be  carried  out  for 
all  sides  of  the  obstacles  to  determine  interference  of  obstacles  with  the 
straight  path  ST. 

One  may  note  that  the  algorithm  for  checking  interference  given 
above  can  be  used  to  check  for  collision  of  a robot  with  obstacles.  Line 
segments  representing  robot  links  are  substituted  for  line  segment  ST  in 
equations  (4.1)  through  (4.9).  This  feature  is  used  for  collision 
prevention  of  a robot  discussed  in  Chapter  5. 


4.2  Geometric  Mapping  of  Two  Dimensional  Obstacles 
4.2.1  Formulation  of  a Parametric  Parabola 

In  this  section,  the  formulation  of  a parametric  parabola  including 
the  method  of  setting  the  ranges  of  parameters  is  explained.  Fig.  4.3 
shows  a hypothetical  workspace  specified  by  a circle  of  radius  r and  a 
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Fig.  4-3  Hypothetical  workspace  and  construction  of  parametric  parabolas 
center,  C,  a start  point,  S,  and  a target  point,  T.  The  midpoint  of  S 
and  T is  denoted  by  M.  The  points  R' , R and  Q are  yet  to  be  obtained. 
The  points  S,  T and  q form  a triangle,  and  F(s)  is  a Bezier  curve  of  order 
two  defined  by  points  S,  T and  q.  The  control  point  q moves  on  the  line 
Lq  and  governs  the  shape  of  parabola.  The  location  of  q is  defined  by  two 
parameters,  9 and  t.  The  parameter  t defines  a pencil  of  parabolas  along 
the  line  L , and  the  parameter  9 , which  defines  families  of  parabolas  along 
a circular  direction,  sets  the  inclination  angle  of  Lq.  The  curve  F(s)* 
is  a Bezier  curve  defined  by  triangle  SQT  and  touches  the  boundary  of  the 
workspace.  The  line  Lq(t')  which  is  defined  by  M and  R'  is  an  intermediate 
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step  before  obtaining  Lq(t)  which  is  defined  by  M and  Q.  The  workspace  has 
to  be  referenced  in  parameter  space  before  carrying  out  the  geometric 
mapping  of  obstacles.  The  range  of  d spans  from  0°  to  360°  and  the  range 
of  t,  yet  to  be  set,  varies  from  0 to  1.  Thus  a circular  workspace  in 
Euclidean  space  is  mapped  into  a rectangular  area  in  parameter  space. 

This  section  addresses  the  method  of  finding  the  point  Q which 
defines  the  largest  parabola  bounded  in  workspace.  When  parabola 
coincides  with  line  ST,  t is  set  to  zero,  and  the  corresponding  control 
point  coincides  with  point  M.  When  parabola  is  in  contact  with  the 
boundary  of  workspace,  t is  set  to  one,  and  the  corresponding  control 
point  is  Q.  The  point  R' , R and  Q have  to  be  obtained  from  the  given 
points  S,  T and  C,  and  the  radius  of  workspace  r,  and  an  inclination  angle 
6 . The  midpoint  of  S and  T is  denoted  by  M,  and  is  the  center  of  rotation 
of  line  Lq.  The  point  M is  specified  by 

M = (S  + T)/2  . (4.14) 

The  point  Rr  is  a rotation  of  T about  M by  angle  6 : 

r V " Mx  + <VMx)cos0 

(4.15) 

L Y = My  + (Ty-My)sin0 

The  line  Lq(t')  defined  by  points  H and  R'  can  now  be  obtained.  The  line 
Lq(t').  in  parametric  form,  is 

r x = <V  - V*'  + Mx 

(4.16) 

L y = (V  ‘ Vtf  + “y 

The  circle  centered  at  C with  radius  r is  expressed  as  follows: 

(x  - Cx)2  + (y  - cy)2  = r2  . (4.17) 

The  point  R is  the  intersection  of  the  circle  and  the  line  Lq(t');  the 
solution  can  be  obtained  in  terms  of  parameter  t'  in  which  t'  should  be 
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greater  than  zero.  By  substituting  (4.16)  to  (4.17)  a quadratic  equation 
in  t'  is  obtained,  and  the  solution  of  t'  is 

t'  = -(ac+bd)  + ( (ac+bd)  2 - (a2+b2  ) (c2+d2-r2  ) )'h 

a2  + b2  <4-18> 

where  a = Rx'  - Mx 

b ’ V ‘ "v 

<=  - "x  - 

d - "y  - °y 

The  point  R can  now  be  obtained  from  (4.16)  by  substituting  the  value  of 
t'  . 

Having  calculated  the  point  R,  the  tangent  line  to  the  circle  at 
R can  be  derived.  The  line  Lt  is  tangent  to  the  circle  and  passes  through 
R,  which  is  expressed  in  implicit  form: 

(X  - RX)(RX  - cx)  + (y  - Ry)(Ry  - cy)  = 0 . (4.19) 

The  main  reason  for  calculating  the  line  Lt  is  now  explained.  For  given 
parameter  value  9 , a bundle  of  parabolas  can  be  generated  by  varying  the 
control  point  on  the  line  L^.  One  remaining  problem  is  to  find  the  largest 
parabola  which  is  bounded  within  the  circle.  A parabola,  F(s)*,  that  is 
in  contact  with  a line  Lt  represents  a close  approximation  to  the  one  that 
is  precisely  in  contact  with  the  circle.  Exact  calculation  for  the 
contact  with  the  circle  is  algebraically  complicated  in  that  a quartic 
equation  in  s is  derived  and  the  coefficients  of  the  equation  contain  t'. 
To  avoid  such  complexity  an  approximation  in  which  the  parabola  is  in 
contact  with  the  line  Lt  is  introduced. 
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The  parabola,  F(s),  defined  by  points  S,q  and  T using  Bezier's 
definition  (3.4)  is 

[■  X - <SX-2<lx+TX>S2  + 2<vsx>s  + SX 

(4.20) 

L 7 - (Sy-2qy+Ty)s2  + 2(qy-Sy)s  + Sy 

where  q,  (qx,  qy) , is  yet  unknown  and,  since  q is  on  the  line  Lq(t'),  q 
is  substituted  by  (4.16).  Thus  (4.20)  can  be  expressed  as  follows: 

- x = a^'s2  - (a.,t'  - b.,)s  + Sx 

(4.21) 

L y = a2t's2  - (a2t'  - b2)s  + Sy 
where  a1  = -2(RX'  - Mx) 
a2  = -2(y  - y 

b1  " Tx  - Sx 
b2  = Ty  - Sy 

The  parameter  tr  indicates  the  location  of  q along  the  line  Lq(t'). 
Eliminating  x and  y from  (4.19)  and  (4.21)  yields 

At ' s2  - (At'  - B)s  + C = 0 (4.22) 

where  A = a.,(Rx-Cx)  + a2(Ry-Cy) 

B - b,(R,-Cx)  + bjC^-C,) 

C - (SX-R>)(R>-CX)  ♦ (Sy-i^H^-C^  . 

Since  the  parabola,  F(s)*,  and  the  line  Lt  is  in  contact,  double  roots  in 
s are  the  only  possible  solutions.  Thus  the  discriminant  of  (4.22) 
should  vanish: 

A2t ' 2 - 2A(B+2C)t ' + B2  = 0 (4.23) 

and  the  meaningful  solution  of  t'  is  the  one  that  is  greater  than  the 

other  (which  is  explained  in  the  next  section) : 

t,  _ A(B+2C)  + 2(A2 (BC+C2))^ 

A2 


(4.24) 
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The  point  Q can  now  be  obtained  by  substituting  the  solution  t'  into 

(4.16) .  The  line  Lq  has  been  defined  by  parameter  t'  which  is  based  on 

points  M and  R' . The  line  Lq  is  then  redefined  by  points  M and  Q and  the 

associated  parameter  is  indicated  by  t as  in  Fig.  4-3.  Hence  R'(R'  R'  ) 

x y 

and  t'  are  replaced  by  Q(Q  , Q ) and  t in  (4.16). 

x y 

One  can  note  that  the  valid  range  of  parameter  t'  of  Lq(t')  varies 
as  the  parameter  9 varies.  By  reformulating  the  line  Lq  with  M and  Q in 

(4.16) ,  where  R'and  t'  are  replaced  by  Q and  t,  the  valid  range  of  t can 
be  set  to  0<t<l.  Thus  the  valid  range  of  parameters  t and  9 are  0<t<l  and 
0°<9<360° . By  varying  parameters  9 from  0°  to  360°  and  t from  0 to  1, 
all  parabolas  within  the  workspace  are  generated.  If  the  range  of  9 and 
t in  which  the  corresponding  parabolas  intersect  with  obstacles  is  plotted 
in  parameter  space  (9,  t) , an  image  of  the  obstacle  is  produced.  This 
process  is  the  geometric  mapping. 

For  given  value  of  9 and  t,  the  point  Q can  be  obtained  following 
the  process  from  (4.14)  to  (4.24)  and  the  corresponding  parabola  can  then 
be  obtained  easily  by  using  (4.16)  and  (4.21)  in  which  R',  t'  should  be 
substituted  by  Q and  t.  Unfavorable  features  of  parabolic  curves  such  as 
sharp  curvature  when  parameter  value  9 is  near  0°  or  180°  should  be 
avoided  during  geometric  mapping.  The  shortest  path  or  the  safest  path 
can  be  extracted  from  the  clear  area  of  the  parameter  space.  The  shortest 
paths  have  the  smallest  value  of  t from  the  clear  area  of  parameter  space 
( 9 , t) , while  the  safest  paths  have  the  widest  clearance  from  neighboring 


edges  of  obstacle  images. 
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4.2.2  Intersection  of  Parabolas  vlth  an  Obstacle 

Fig.  4-4  shows  an  obstacle  and  a bundle  of  parabolas.  It  is  clear 
that  the  obstacle  limits  the  valid  range  of  parameter  t.  There  are  two 
cases  of  obstacle  interference  limiting  the  range  of  parameter  t.  One  is 
a contact  with  a vertex  of  the  obstacle  and  the  other  is  a contact  with 
an  edge  of  the  obstacle.  For  instance,  in  Fig.  4-4,  vertex  P3  is  in 
contact  with  parabola  F2(s),  and  edge  P,jP2  is  in  contact  with  parabola 
F^s).  Because  parameter  t1  generates  F.,(s)  and  t2  generates  F2(s),  the 
issue  is  how  to  calculate  t1  and  t2.  Additionally  it  has  to  be  decided 
among  which  of  the  intervals  0<t<t1 , t^t^tg  and  t^til  parabolas  intersect 
the  obstacle.  This  section  addresses  the  calculation  of  parameter  value 
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t1  and  t2,  and  the  determination  of  parameter  interval  is  explained  in 
Section  4.2.3. 

Contact  between  an  obstacle  vertex  and  a parabola 

The  vertex  P3  is  contact  with  parabola  F2,  Fig.  4-4.  Expressions 
for  the  parabola,  which  is  essentially  the  same  as  (4.21),  is  repeated 
here  with  minor  changes  in  coefficients  a1  and  a2  where  Rx'  and  Ry'  are 
replaced  by  Q and  Q , respectively, 

a y 

- x = a^s2  - (a.,t  - b^s  + Sx 
y = a2ts2  - (a2t  - b2)s  + S 


(4.25) 


where  a 


1 ~ -2«x  - Mx> 


a2  = -2(Qy  • V 

b1  = Tx  - Sx 

b2  = Ty  - Sy 

Since  the  parabola  is  defined  in  terms  of  parameter  t,  t2  for  F2  can  be 
calculated  by  substituting  P3  into  (4.25).  Elimination  of  t from  both 
equations  yields 

(al'(P3y  ' Sy>  ' a2‘<P3x  " Sx^ 


s = 


(4.26) 


(a1 • b2  - a2 • b1 ) 

Parameter  s represents  a point  along  the  parabola.  Parameter  t is 
obtained  by  substituting  (4.26)  into  (4.24)  or  (4.25)  provided  that  the 
denominator  of  either  (4.24)  or  (4.25)  does  not  vanish.  If  the  solutions 
of  s and  t are  0<s<l  and  0<t<l , contact  between  vertex  P3  and  parabola  F2 
occurs . 

Contact  between  an  obstacle  edge  and  a parabola 


The  line  segment  defined  by  P1  and  P4  contacts  parabola  F1 . 
Calculation  of  parameter  t1  is  similar  to  the  one  given  previously  in 
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Section  4.2.1  where  the  contact  between  a parabola  and  line  Lt  yields  a 
solution  for  t in  (4.24).  The  edge  defined  by  vertices  P1  and  P4,  in 
parametric  form,  is 

T X = (P1x-P4x>'U  + P4x 


(4.27) 


y “ <W'U  + P4y 

Elimination  of  x and  y from  equations  (4.25)  and  (4.27)  yields 
' <P1x-p4x>‘u  + P4x  " art-sJ  - (a,  • t-b,)  • s + Sx 
^ (P1y'P4y^  "U  + p4y  = &2’t'S2  - (a^t-b^'S  + Sy 

and  again  elimination  of  u from  (4.28)  yields 
Ats2  + (-At  + B)s  + C = 0 
where  A - (P,y  - P4y)a,  - (P,„  - P4ll>a2 

B - <P1y  - P4y>b1  - <P,x  - P4x>b2 

C - <Ply  - P4yXS„  - p4x>  - (P„  - P4>)(Sy  - p4y) 

Equation  (4.29)  has  double  roots  in  s,  since  the  parabola  is  just  in 

contact  with  the  edge  of  the  obstacle.  Thus  the  discriminant  of  (4.29) 
will  vanish: 


(4.28) 


(4.29) 


A2t2  - 2(AB  + 2AC)t  + B2  = 0 


(4.30) 


P4  Pi 


Fig.  4-5  Two  possible  cases  of  contact  between  a line  and  a parabola 
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Equation  (4.30)  may  have  two  different  solutions  for  t as  shown  in  Fig. 
4-5  in  which  F1  and  F2  are  the  corresponding  curves.  It  reveals  that  the 
root  for  F2  approaches  zero  and  is  always  smaller  than  the  other  root  for 
F,|.  The  root  for  F1  is  the  valid  solution  due  to  the  fact  that  the  line 
segment  considered  here,  P^,  is  bounded  by  the  triangle  SQT,  Fig.  4-4. 
Hence  t is  given  by 

- - A(B+2C)  + 2(A2(BC+C2))*  . 


Having  calculated  t,  the  corresponding  s can  be  obtained  by  substituting 

(4.31)  to  (4.29), 

AC-t  (AS(BC+C2))^ 

A(B+2C)  + 2(A2(BC+C2))*  * 


The  parameter  u can  now  be  calculated  from  one  of  the  equations  of (4. 28) 
provided  that  the  denominator  does  not  vanish.  If  the  values  of  s,  t and 
u are  all  between  zero  and  one,  a contact  occurs.  If  edge  is  parallel 

to  the  line  Lq(t),  the  denominator  of  (4.31)  vanishes  and  t becomes 
infinite.  This  special  case  is  considered  in  the  following  section. 
Special  cases 

When  Q lies  on  line  ST,  the  parabola  collapses  to  a straight  line 
which  is  collinear  with  line  ST.  This  case  is  discussed  in  Section  4.1 
in  which  intersections  of  the  straight  path  ST  with  edges  of  obstacles  are 
considered . 

When  considering  a contact  of  a vertex  or  an  edge  with  parabolas, 
singular  cases  associated  with  the  values  of  s and  t,  such  as  t=0,  t=®  and 
t or  s being  indeterminate  may  arise.  If  an  edge  of  an  obstacle  under 
consideration  lies  on  the  line  G(u),  B and  C vanish  from  equation  (4.29), 
thus  t becomes  zero  from  (4.31)  and  s becomes  indeterminate  from  (4.32). 
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Fig.  4-6  Special  cases 

The  intersection  can  be  checked  by  considering  the  vertices  of  the  edge. 
If  one  or  both  of  the  vertices  are  inside  of  line  segment  ST,  intersection 
occurs . 

If  an  edge  under  consideration  lies  on  the  line  L1  or  L2  in  which 
they  are  parallel  to  Lq  and  pass  through  S or  T,  the  coefficient  A vanishes 
from  (4.29)  and  t becomes  infinite  from  (4.31).  This  situation  can  be 
avoided  by  evaluating  the  value  A first,  and  determining  the  case.  A 
simple  and  general  method  to  avoid  this  situation  is  to  compare  the 
denominator  with  the  numerator  at  the  beginning.  If  the  numerator  is  less 
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than  the  denominator,  value  of  t from  (4.31)  is  in  the  range  -l<t<l  and 
it  should  be  evaluated,  otherwise  the  evaluation  can  be  skipped.  If  t is 
in  the  range  of  0<t<l,  an  actual  intersection  accurs . 

If  a vertex  under  consideration  lies  on  the  line  L,  or  Lj,  s becomes 
0 or  1,  respectively,  from  the  equation  (4.26)  and  t becomes  infinite  from 
equation  (4.24)  and  (4.25).  If  a vertex  point  is  coincident  with  S or  T, 
t becomes  indeterminate.  Such  cases  can  be  avoided  by  comparing  the 
vertex  point  with  S and  T. 

4.2.3  Ranees  of  t by  each  Obstacle 

Obstacles  in  the  workspace  limit  the  range  of  parameter  t.  Since 
the  calculation  of  the  intersection  between  an  obstacle  and  the  triangle 
SQT  is  much  simpler  than  that  of  between  an  obstacle  and  parabolas, 
obstacles  which  intersect  or  are  contained  entirely  within  the  triangle 
SQT  can  be  sorted  out  to  selectively  apply  previously  derived  formulae  in 
Section  4.2.2.  Thus  the  computation  to  get  the  ranges  of  t can  be  reduced 
significantly. 

Fig.  4-7  shows  all  possible  cases  of  interferences  between  triangle 
SQT  and  an  obstacle.  The  calculation  of  the  intersection  between  an  edge 
of  an  obstacle  and  an  edge  of  triangle  SQT  is  carried  out  by  using 
equations  (4.3),  (4.4)  and  (4.5). 

In  case  (a)  of  Fig.  4-7,  the  obstacle  does  not  intersect  the  line 
segment  ST,  SQ  and  QT,  hence  it  may  be  inside  or  outside  of  the  triangle 
SQT.  It  can  be  determined  by  the  signs  of  (4.5)  in  which  points 
representing  S,  T and  PQ  should  be  substituted  appropriately.  For  example, 
a vertex  PQ  is  determined  inside  if  |STP0|,  |QSP0|  and  |TQP0|  are  positive. 
If  signs  for  all  vertices  are  positive,  it  is  inside  of  SQT,  otherwise  it 
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(a)  (b) 


(c)  (d) 


Fig.  4-7  Possible  cases  of  obstacle  interference 
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(e)  (f) 


(g)  00 


Fig.  4-7--continued. 
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is  outside  of  SQT.  This  case  is  outside  of  SQT,  and  does  not  require 
calculation  of  the  range  of  t because  the  obstacle  is  outside  of  SQT. 

In  case  (b)  of  Fig.  4-7,  the  obstacle  intersects  line  segment  SQ, 
hence  the  interference  range  of  t of  the  parabolas  is  t*<t<l.  It  requires 
calculation  of  both  point  and  line  contact  between  parabola  and  obstacle, 
which  is  given  in  the  previous  section.  An  obstacle  intersecting  line 
segment  QT  can  be  treated  in  the  same  way. 

In  case  (c)  of  Fig.  4-7,  the  obstacle  intersects  both  line  segment 
SQ  and  ST,  therefore,  the  interference  range  of  t of  the  parabolas  is 
t*<t<l.  Calculation  is  the  same  as  case  (b) . 

In  case  (d)  of  Fig.  4-7,  the  obstacle  intersects  both  line  segment 
ST  and  SQ,  therefore,  the  entire  parabolas  intersect  with  the  obstacle. 
An  obstacle  intersecting  both  line  segments  ST  and  TQ  can  be  treated  in 
the  same  way. 

In  case  (e)  of  Fig.  4-7,  the  obstacle  does  not  intersect  the  line 
segments  ST,  SQ  or  QT,  rather  it  is  contained  in  the  triangle  SQT.  The 
equation  (4.5)  can  be  used  to  determine  whether  the  obstacle  is  contained 
in  the  triangle  SQT.  A proper  substitution  of  vertex  to  equation  (4.5) 
is  necessary  to  determine  the  case.  The  interference  range  of  t of  the 

4r 

parabola  is  t1  <t<t2  . It  requires  calculation  of  both  point  and  line 
contact  between  parabola  and  obstacle. 

In  case  (f)  of  Fig.  4-7,  the  obstacle  does  not  intersect  any  line 
segment  ST,  SQ  or  QT.  It  is  similar  to  case  (e)  except  that  the  resulting 
value  of  t2*  is  greater  than  one,  so  the  interference  range  of  t is  t*<t<l. 

In  case  (g)  of  Fig.  4-7,  the  obstacle  intersects  line  segment  ST 
only.  The  interference  range  of  t of  the  parabolas  is  0<t<t*.  It  requires 
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computation  of  point  contact  between  the  vertices  that  are  inside  the 
triangle  SQT  and  parabolas . 

In  case  (h)  of  Fig.  4-7,  the  obstacle  intersects  line  segment  ST 
only.  It  is  similar  to  case  (g)  except  that  the  parameter  t*  is  greater 
than  one,  so  all  the  parabolas  intersect  with  the  obstacle. 

If  more  than  one  obstacle  is  inside  the  workspace  and  the  family 
of  parabolas  intersects  with  more  than  one  obstacle,  union  of  the  range 
of  t representing  intersection  by  each  obstacle  is  the  actual  intersection 
of  parabolas  with  all  obstacles  inside  the  workspace.  Fig.  4-8  shows  the 
intersection  of  parabolas  with  two  obstacles,  where  9*  indicates  the  family 
of  parabola.  The  ranges  of  t in  which  parabolas  interfere  with  obstacle 
A and  B are  0<t<t1  and  t-^tltj.  Thus  the  valid  range  of  t is  t,,<t<t2  and 
^3— t— !•  Because  the  number  of  obstacles  that  intersect  with  parabolas 
varies  as  the  family  of  parabolas,  which  depends  on  parameter  0,  varies, 
the  routine  for  finding  the  valid  range  of  t should  cope  with  any  number 
of  obstacles. 

The  process  described  so  far  creates  ranges  of  t,  in  which 
parabolas  intersect  with  obstacles  A and  B,  for  given  values  of  9*.  In 
Fig.  4-9,  the  dashed  line  corresponds  to  9*  and  values  of  t1 , t2  and  t3 
are  plotted  accordingly.  To  create  whole  images  of  obstacles  in  parameter 
space,  ranges  of  t for  different  values  of  9 have  to  be  calculated.  Such 
a process  creates  images  in  parameter  space  as  shown  in  Fig.  4.9. 

4.2.4  Selection  of  a Collision-Free  Parabola 

The  process  of  creating  images  of  obstacles  in  parameter  space  is 
called  geometric  mapping.  One  should  note  that  geometric  mapping  for  the 
entire  range  of  9 is  not  necessary  because  the  values  of  9 near  0°  and 
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Fig.  4-8  Interference  range  of  t due  to  two  obstacles 
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180°  produce  sharp  parabolas  and  the  valid  range  of  t is  much  smaller  than 
for  other  values  of  6 . 

Any  point  from  the  clear  area  of  parameter  space  corresponds  to  a 
collision-free  parabolic  path.  It  is  apparent  that  selecting  a point  that 
is  farthest  away  from  boundaries  of  all  obstacle  images  gives  the  safest 
path.  The  ranges  of  t for  each  different  value  of  6 are  compared  to 
determine  the  widest  range  of  t,  and  the  mid  value  of  that  range  is 
selected  as  the  value  of  t.  This  pair  is  only  an  approximation  of  the 
safest  path,  since  the  comparison  is  made  in  6 -direction  only.  Search  for 
the  safest  path  may  not  be  necessary  because  any  point  in  the  clear  area 
of  the  parameter  space  is  already  a collision-free  parabolic  path.  If 
obstacles  are  not  exactly  where  they  should  be,  the  choice  of  path  should 


be  the  safest  one. 
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It  is  also  possible  to  select  the  shortest  path  in  terms  of 
parabolic  path  by  searching  for  a point  that  has  the  smallest  value  of  t. 
In  essence  geometric  mapping  converts  the  problem  of  selecting  a path  in 
Euclidean  space  to  the  problem  of  selecting  a point  in  parameter  space, 
which  is  obviously  an  easier  problem.  The  method  of  selection  can  be 
changed  depending  on  the  application. 

As  an  example,  the  process  of  selecting  the  shortest  path  is  now 
explained.  Initially,  a vertical  line  is  drawn  in  parameter  space  at  some 
value,  for  example  6*,  and  the  smallest  value  of  t is  found,  t1 , from  a 
vertical  line-segment  that  belongs  to  the  clear  area.  By  increasing  the 
value  of  6 , another  value  of  t can  be  obtained  and  compared  to  the 
previous  value  t1 . The  pair  (0,t)  having  smaller  value  of  t is  stored. 
This  process  is  continued  until  0*+36O°,  and  the  pair  ( 6 ,t ) having  the 
smallest  value  of  t is  obtained.  By  substituting  6 to  (4.15),  R'(R'  ,R'  ) 
is  obtained  and  substitution  of  R'  and  t to  (4.21)  yields  the  shortest 
parabola . 


4 . 3 Composite  Path 

Fig.  4-10  shows  a situation  in  which  obstacles  in  the  workspace  do 
not  permit  a parabola  to  pass  from  S to  T without  intersections.  In  such 
a case  parameter  space  will  be  filled  with  obstacle  images.  It  is  then 
required  to  use  a higher  degree  Bezier  curve  other  than  a parabolic  curve, 
however,  as  the  degree  of  curve  increases  so  does  the  number  of  control 
points  that  define  the  curve.  Since  each  control  point  associates  two 
parameters  the  total  dimension  of  parameter  space  increases  to  two  times 
the  number  of  control  points.  Theoretically,  the  geometric  mapping  in 
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dimensions  higher  than  two  is  possible,  but  it  is  computationally  too 
complicated  to  implement. 

An  alternative  to  employing  a higher  order  curve  for  geometric 
mapping  is  to  connect  parabolic  curves  in  series  and  smooth  the  connection 
points.  Thus  the  original  problem  is  reduced  to  one  of  finding 
intermediate  connection  points,  and  the  smoothing  of  the  slope 
discontinuities  at  connection  points.  Once  intermediate  points  are  found, 
the  geometric  mapping  is  applied  to  get  a collision-free  parabola  for  each 
subinterval.  Slope  discontinuity  at  connection  points  between  two 
parabolas  can  be  made  smooth  after  connecting  collision-free  parabolas. 
These  issues  are  discussed  in  the  following  subsections. 

4.3.1  Selection  of  Intermediate  Connection  Points 

A connection  point  cannot  be  selected  arbitrarily.  It  should  be 
within  the  workspace  and  outside  of  obstacles.  It  is  obvious  that  a 
connection  point  should  be  selected  in  such  a way  that  the  resulting  path 
should  circumvent  at  least  those  obstacles  that  block  the  straight  path 
from  start  to  target  point.  Intermediate  points  can  be  selected  at 
surrounding  areas  of  those  obstacles  and  geometric  mapping  is  carried  out 
for  each  subinterval  to  obtain  a collision-free  path.  Fig.  4-10  shows  a 
method  to  select  an  intermediate  connection  point.  The  vertices  P2  and 
P3  are  on  one  side  and  P1  and  P4  are  on  the  other  side  of  line  ST.  P2,  P3 
and  P1 , P4  are  viable  points  for  paths  on  the  upper  and  lower  sides, 
respectively.  P2  and  P1  are  closer  to  the  perpendicular  bisector  of  ST, 
so  connection  points  selected  around  P2  and  P1  tend  to  produce  paths  of 
similar  length.  Although  an  intermediate  connection  point  is  selected 
near  vertex  P2  and  the  resulting  path  is  close  to  the  vertex,  the  smoothing 
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Fig.  4-10  Selection  of  intermediate  connection  point 

process  yields  a path  that  has  clearance  between  path  and  obstacle  as 
shown  in  Fig.  4.14.  This  point  will  be  explained  in  section  4.3.3. 

4.3.2  Four  Fundamental  Modes  of  Paths 

In  general,  it  is  desirable  to  classify  most  fundamental  ways  of 
connection  regardless  of  the  number  of  intermediate  connection  points. 
Thus  implementation  of  the  algorithm  can  be  simplified.  Four  fundamental 
paths  are  shown  in  Fig.  4-11.  Another  reason  for  the  categorization  is 
to  give  room  for  the  smoothing  process  so  that  the  possibility  of 
interference  between  the  altered  portion  of  the  path  and  obstacles  is 
reduced  as  much  as  possible. 
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(b) 

Fig.  4-11  Four  fundamental  mode  of  paths 
(a)  Mode  la;  (b)  Mode  lb;  (c)  Mode  Ha;  (d)  Mode  lib; 
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Fig.  4-11- -continued. 
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The  four  fundamental  modes  of  paths  are  mode  la,  mode  lb,  mode  Ila 
and  mode  lib.  Subintervals  of  fundamental  modes  of  paths  consist  of 
collision-free  paths.  The  collision-free  paths  for  subintervals  are  the 
results  from  geometric  mapping.  In  the  process  of  geometric  mapping,  the 
parameter  range  of  8 is  divided  into  two  ranges  O°<0<18O°  and  18O°<0<36O°. 
A parabola  with  low  value  of  8 indicates  the  range  of  9 being  O°<0<18O° 
and  a parabola  with  high  value  of  8 indicates  the  range  of  8 being 
18O°<0<36O° . In  mode  la,  two  parabolas  with  low  values  of  6 are  connected 
in  a series.  In  mode  lb,  two  parabolas  with  high  values  of  8 are 
connected  in  a series.  In  mode  Ila,  a parabola  with  low  value  of  8 and 
a parabola  with  high  value  of  9 are  connected  in  a series.  In  mode  lib, 
a parabola  with  high  value  of  8 and  a parabola  with  low  value  of  8 are 
connected  in  a series. 

It  is  also  critical  to  check  if  the  selected  intermediate  point  is 
inside  of  any  other  obstacle.  If  it  is,  the  selected  intermediate 
connection  point  should  be  discarded.  If  a collision-free  path  for  a 
subinterval  by  a fundamental  mode  is  found  to  be  impossible  to  create, 
other  modes  are  employed.  It  is  also  possible  that  all  path  modes  fail 
to  generate  a collision-free  path  for  a certain  subinterval.  Such  a case 
indicates  that  geometric  mapping  for  that  subinterval  does  not  provide  a 
collision-free  path.  Therefore  it  has  to  be  divided  into  smaller 
sub intervals . 

There  are  variations  of  mode  I and  mode  II  to  provide  additional 
modes  for  the  case  when  the  chosen  mode  fails  to  generate  collision-free 
paths.  The  purpose  of  additional  modes  is  to  prevent  unnecessary  division 
of  the  subinterval.  Fig.  4-12  shows  three  alternative  paths  for  mode  la 
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Fig.  4-12  Variations  of  mode  la 


Fig.  4-13  Variations  of  mode  Ila 
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where  more  than  two  obstacles  are  across  the  line  segment  ST  and  an 
intermediate  connection  point  is  selected  from  each  obstacle.  Fig.  4-13 
shows  two  alternative  paths  for  the  mode  Ila,  where  two  intermediate 
points  are  selected  at  the  center  of  line  segments  a^  and  a2b1 . 

^•3.3  Smoothing  Slope  Discontinuity  at  Connection  Point 

If  a collision-free  path  so  constructed  has  a slope  discontinuity 
at  a connection  point,  smoothing  is  desirable.  Four  different  methods  of 
making  a smooth  connection  of  two  parametric  curves  are  described  in 
Section  3.1.4.  Of  the  four  methods  of  smoothing  slope  discontinuity, 
connection  by  combination  of  portions  of  neighboring  curves,  equations 
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Fig.  4.14  Smoothing  slope  discontinuity 
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(3.20)  through  (3.24),  and  connection  by  a quartic  curve,  equations  (3.35) 
through  (3.39),  are  adopted  here.  For  mode  I paths,  smoothing  by 
combination  of  portions  of  neighboring  curves  is  better  because  it  gives 
more  clearance  from  obstacles  than  other  methods,  as  shown  in  Fig.  4-14. 
For  mode  II  paths,  both  methods  can  be  used,  because  unsmoothed  path  has 
a relatively  smooth  connection. 

One  should  recognize  that  the  smoothing  by  combination  of  portions 
of  neighboring  curves  can  destroy  the  collision-free  nature  of  the 
original  path.  The  line  segment  C.jC2  is  used  to  check  for  obstacle 
interference.  If  it  intersects  with  any  obstacle,  then  the  portions  C^?2 
and  C2P2  are  reduced  until  no  interferences  occurs  between  the  line  segment 
C^C2  and  obstacles. 

4 . 4 Collision-Free  Paths  in  Three  Dimensions 

Key  ideas  for  planning  collision-free  paths  in  three  dimensions 
were  explained  in  Section  3.3.  It  is  assumed  that  obstacles  are  convex 
hulls  and  consist  of  flat  surfaces,  for  example  a cylinder  is  represented 
by  a polygonal  prism.  A comprehensive  treatment  on  the  topics  related  to 
convex  hulls  can  be  found  in  a book  by  Preparata  and  Shamos (1985) . 

The  flat  projection  plane  may  be  defined  by  three  points,  start, 
target  and  the  origin  of  the  global  coordinate  system.  A local  coordinate 
system  can  be  set  on  the  projection  plane.  Fig.  4-15.  The  x and  y axes 
of  the  local  coordinates  are  coplanar  with  the  projection  plane.  The 
parameter  </>  indicates  the  amount  of  rotation  about  the  x axis.  A 
transformation  matrix.  A,  which  transforms  a vector  between  the  local  to 
the  global  coordinate  systems,  can  be  expressed  in  terms  of  unit  vectors 
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Fig.  4-15  Intersection  of  an  obstacle  with  flat  projection  planes 


Fig.  4-16  Image  of  obstacle  on  the  projection  plane  defined  by  a 
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that  lie  along  the  x,  y and  z axes  of  local  coordinates. 


x kx 


j 

Jl 


(4.33) 


where  i = (ix  iy  iz)  , j = (jx  jy  jk)  and  k = (kx  ky  kz)  are  the  unit  vectors. 
Since  the  matrix  A itself  is  a orthogonal  matrix,  transpose  of  A,  AT, 
transforms  a vector  from  global  coordinates  to  the  local  coordinate 
system. 

There  are  two  ways  of  calculating  intersection  of  obstacles  with 
the  projection  plane.  One  is  to  carry  out  the  transformation  of  all 
vertices  to  the  local  coordinate  system,  then  calculate  the  intersections 
with  the  x-y  plane  of  the  local  coordinate  system.  The  other  is  to 
determine  intersections  of  obstacles  with  the  projection  plane  expressed 
in  global  coordinates.  The  resulting  intersection  points  are  transformed 
to  local  coordinates  to  exclude  the  z component.  Computation  of 
intersections  has  to  be  done  for  all  edges  of  the  obstacles. 

As  an  illustration,  the  process  of  obtaining  point  P1  of  Fig.  4-16 
is  given  as  follows . The  edge  connecting  vQ  and  v1  intersects  the 
projection  plane  and  the  intersection  point  is  P1 . Transformation  of  vQ 
and  v1  in  global  coordinates  to  local  coordinates  can  be  done  by 
multiplying  the  transformation  matrix  AT  to  obtain 


(4.34) 


V1  = A V1 


The  edge  connecting  v0‘  and  v1  ‘ , in  parametric  form,  is 
x(t)  = (l-t)v0'x  + tv^ 
y(t)  = (i-t)v0‘y  + tv, 1 y 


(4.35) 
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z(t)  = (l-t)v0'z  + tv,'z 

and  the  projection  plane  expressed  in  local  coordinates  is 

z = 0 (4.36) 

since  the  x and  y axes  are  attached  to  the  projection  plane.  Thus  the 
edge  and  projection  plane  are  both  expressed  in  the  local  coordinate 
system.  By  substituting  (4.36)  into  (4.35)  the  parameter  t can  be 
obtained: 


'0  z 


t = 

I I 


(4.37) 


v0  z - V1  z 

Substitution  of  (4.37)  into  (4.35)  yields  the  coordinates  of  intersection 
point  P1 . 

Obstacle  images  on  the  projection  plane,  however,  are  not  yet 
complete.  The  computation  merely  provides  intersection  points  between 
edges  of  obstacles  and  the  projection  plane.  These  points  have  to  be 
organized  to  form  a convex  polygon,  because  intersection  of  a convex  hull 
with  a flat  plane  produces  a convex  polygon.  A method  of  forming  a convex 
polygon  from  those  points  on  the  projection  plane  is  now  explained  for 
Fig.  4-16  as  an  example.  An  indepth  treatment  on  this  subject  can  be 
found  in  Preparata  and  Shamos  (1985).  Vertex  numbering  in  Fig.  4-16 
indicates  the  sequence  of  calculation  of  intersections  which  depends  on 
the  numbering  of  vertices  of  Fig.  4-15.  Point  P1  and  P2  are  the  reference 
points  and  form  a line.  Remaining  points  are  divided  into  two  groups 
depending  on  if  they  are  on  the  left  or  right  side  of  the  reference  line. 
Two  points  from  one  side  are  selected  again  as  reference  and  the  division 
separates  remaining  points  into  two  groups  again.  This  process  stops  when 
the  number  of  points  on  one  side  is  less  than  two.  For  instance,  in  Fig. 
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4-16  point  P1  and  P2  are  reference  points  and  points  P3  and  P4  are  on  the 
left  side.  Point  P4  is  on  the  right  side  with  respect  to  the  line  drawn 
between  points  P1  and  Pj.  The  points  are  stored  in  a tree  structure. 
Points  retrieved  from  left  to  right  of  the  tree  structure  are  organized 
in  a clockwise  fashion;  those  from  right  to  left  are  ordered  in  an 
anticlockwise  manner.  The  final  result  can  be  either  in  the  sequence  P1 , 
**3’  ^4*  **2  or  **1 ' **2*  ^4»  The  image  can  now  be  fed  into  the  geometric 
mapping. 

To  get  a collision-free  path  in  three  dimensional  environments, 
control  points  for  a two  dimensional  collision-free  path,  P in  Fig.  4-16, 
have  to  be  converted  back  to  three  dimensions.  This  can  be  done  by 
multiplying  the  two  dimensional  control  points  by  transformation  matrix 
A.  This  reverses  the  process  transforming  a vertex  from  global  to  local 
coordinates.  Translations  between  the  origins  of  the  three  dimensional 
workspace  and  the  two  dimensional  workspace  should  be  properly  set  to 
match  the  three  dimensional  collision-free  path. 

4.5  Implementation  of  the  Algorithms 

Shown  in  Fig.  4-17  is  an  overall  flow  of  logic  for  finding  a two 
dimensional  collision-free  path.  Search  for  a collision  free-path  stops 
when  either  a successful  path  is  found  or  the  recursion  depth  becomes  zero 
which  indicates  a failure.  For  three  dimensional  environments,  a section 
for  finding  two  dimensional  images  of  obstacle  on  projection  plane  should 
be  placed  just  after  the  input  data. 

Algorithms  are  implemented  on  a VAX  11/750  computer  and  a Silicon 
Graphics  IRIS  4D/70  graphics  workstation.  The  program  is  developed  in  C 
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Fig.  4-17  Flow  chart  of  two  dimensional  collision-free  path  algorithm 
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language  due  to  the  portability  between  the  VAX  11/750  and  the  Silicon 
Graphics  IRIS  4D/70.  Input  parameters  for  the  program  are  origin,  start 
and  target  points,  radius  of  the  work  space,  coordinates  of  obstacle 
vertices  and  the  expansion  length.  Obstacle  information  is  stored  in 
structure  variables,  a feature  in  C language,  and  each  obstacle  is 
organized  in  a linked  list  so  that  the  number  of  obstacles  and  the  number 
of  vertices  in  an  obstacle  are  not  limited.  Adding  an  obstacle, 
therefore,  is  accomplished  by  adding  the  information  to  the  data  file. 

Simulation  results  are  given  in  Appendix  A for  three  different 
environments:  randomly  scatered  obstacles,  obstacles  resembling 
compartments  and  obstacles  resembling  rooms  and  corridors.  Simulations 
were  carried  on  VAX  11/750  and  ploted  using  Hewelett-Packard  7475A 
plotter.  Confining  walls  are  represented  by  obstacles  too,  hence  the 
rectangular  area  inside  of  the  walls  represents  the  actual  work  area. 
Variously  shaped  polygons  represent  obstacles  and  dash-dot  lines  indicate 
expanded  obstacles.  When  a collision-free  parabola  between  start  and 
target  points  is  not  found,  intermediate  connection  points  are  searched. 
A collision-free  path  for  a sub- interval  is  plotted  with  a dash-dot  line. 
The  final  path  after  the  smoothing  is  plotted  with  a solid  line. 

Execution  time  of  the  program  depends  on  several  factors:  total 
number  of  obstacles,  location  and  shape  of  obstacles,  algorithm 
implementation  methodology,  and  computer  to  be  used.  Approximate  figure 
of  execution  time  for  the  plots  given  in  Appendix  A is:  O.l(sec)  for  Fig. 
A-l , 0.3  (sec)  for  Figures  A-2,  A-3  and  A-7,  0.5  (sec)  for  Figures  A-4, 
A-5  and  A-6,  1.0  (sec)  for  Figures  A-8  through  A-ll. 
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4. 6 Concluding  Remarks 

In  this  chapter  the  detailed  algorithms  for  planning  collision-free 
paths  have  been  studied.  A straight  path  connecting  the  start  and  target 
point  is  examined  at  the  beginning  to  check  if  any  obstacle  is  across  the 
path.  If  it  is,  a two  dimensional  geometric  mapping  based  on  parametric 
parabola  is  conducted  and  images  of  obstacles  in  parameter  space  is 
constructed.  To  have  a proper  relationship  between  Euclidean  space  and 
parameter  space,  the  ranges  of  parameters  have  to  be  set  initially. 
Control  points  of  a parabola  are  organized  in  polar  coordinates  and  ranges 
of  8 and  t are  set  to  O°<0<36O°  and  0<t<l.  For  varying  values  of  8, 
intersections  of  parabola  with  obstacles  are  calculated  in  terms  of  t. 
This  produces  images  of  the  obstacle  in  parameter  space  and  a point  from 
the  clear  area  of  parameter  space  corresponds  to  a collision-free 
parabola.  If  parameter  space  is  filled  with  images  of  obstacles,  a 
collision-free  parabola  connecting  start  and  target  point  does  not  exist. 
At  this  point,  instead  of  using  higher  order  curves,  intermediate 
connection  points  are  determined.  Then,  the  geometric  mapping  for  the 
subsections  divided  by  an  intermediate  point  is  carried  out. 
Subsequently,  smoothing  the  slope  discontinuity  at  the  connection  point 
is  conducted. 

For  collision-free  paths  in  three  dimensional  environments,  a 
straight  path  connecting  start  and  target  point  is  examined  at  the 
beginning  to  check  if  any  obstacle  is  across  the  path.  If  the  straight 
path  intersects  an  obstacle,  a three  dimensional  geometric  mapping  is  used 
to  obtain  a collision-free  path.  Three  parameters,  </> , 8 and  t,  are 
involved  in  three  dimensional  parametric  parabolas.  To  get  a three 
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dimensional  geometric  mapping,  two  dimensional  geometric  mapping  is 
applied  to  the  direction  of  <j> . In  other  words,  by  varying  the  value  of 
<t>  and  applying  two  dimensional  geometric  mapping  in  the  6- 1 plane  a three 
dimensional  geometric  mapping  results. 

Geometric  mapping  based  on  a curve  of  degree  higher  than  two  is  not 
fully  investigated  in  this  dissertation.  Parameter  space  produced  by  such 
a geometric  mapping  may  ultimately  be  filled  with  images  of  obstacles  when 
the  workspace  is  cluttered,  therefore,  requiring  intermediate  connection 
points.  Higher  order  curves,  however,  reduce  the  necessity  of  finding 
intermediate  connection  points . 


CHAPTER  5 

COLLISION  AVOIDANCE  OF  INDUSTRIAL  ROBOTS 
IN  CONJUNCTION  WITH  PLANNING  COLLISION-FREE  PATHS 

In  this  chapter  problems  associated  with  the  collision  avoidance 
of  a planar  3R  robot  and  a General  Electric  P60  robot  are  discussed. 
Collision  avoidance  here  refers  to  a class  of  problems  in  which  a path  for 
the  robot  is  predefined  and  the  robot  maneuvers  to  escape  collision. 
Another  class  of  problems  are  that  the  start  and  target  points  are  given 
and  the  robot  maneuvers  to  avoid  collision.  Work  related  to  the  first 
can  be  found  in  Young  and  Duffy  (1987),  in  which  a planar  3R  robot  was 
examined.  In  this  the  robot  avoids  obstacles  by  obtaining  the  allowable 
range  of  orientation  for  every  step  along  the  path  and  changing  the 
orientation  to  the  mid-value  of  the  allowable  range.  A second  can  be 
found  in  Brooks  (1983b),  in  which  a PUMA  robot  was  examined.  The  first 
three  joints  of  the  PUMA  robot  were  assumed  active.  A collision-free  path 
is  obtained  in  joint  space1  which  ensures  collision-free  motion  of  the 
robot  in  Euclidean  space. 

The  first  class  of  problem  is  considered  in  this  chapter.  A path 
is  assumed  to  be  given  by  previously  developed  path  planning  algorithm. 
Several  algorithms  which  include  collision  recognition,  obstacle  expansion 
and  change  of  configuration  are  developed  for  collision  avoidance.  The 

1 The  dimension  of  a joint  space  is  the  same  as  the  number  of  joints 
the  robot  has.  A 3R  robot  has  a three-dimensional  joint  space. 
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assumptions  made  are  that  orientation  of  the  end-effector  can  be  altered 
along  the  path,  however,  position  cannot  be  changed  during  the  change  of 
configuration. 

One  of  the  main  objectives  of  this  chapter  is  to  develop  a method 
for  identifying  collision  of  a robot.  It  is  desirable  to  identify  a 
collision  before  the  robot  moves  to  the  next  point.  Obstacles  that  may 
be  machines,  workpieces,  machine  operators,  other  robots  or  any  objects 
inside  the  robot  workspace  reduce  the  actual  workspace  of  the  robot. 
Related  work  to  the  analysis  of  robot  workspace  may  be  found  in  Kumar,  A. 
and  Waldron  (1981),  Sugimoto  and  Duffy  (1981)  and  Davidson  and  Hunt 
(1987)  . The  motion  of  a robot  should  be  restricted  in  a designated  space 
to  avoid  accidents.  Collision  recognition  has  not  been  incorporated  in 
robot  control  loops,  rather  it  is  left  to  robot  operators.  Hence 
operators  have  to  consider  preventing  collisions  in  controlling  robots. 
Collision  recognition  may  be  added  to  existing  robot  control  loops  as  an 
option  to  help  reduce  the  programming  time  as  well  as  prevent  accidents. 
A related  work  on  collision  recognition  can  be  found  in  Crane  (1987)  in 
which  one  obstacle  (a  sphere  or  a cube)  was  assumed.  Collision  detection 
was  carried  out  in  real  time  utilizing  clipping  hardware  of  a graphics 
computer  (Silicon  Graphics  IRIS  series  computer)  in  which  the  obstacle  was 
replaced  by  a bounding  box.  Any  attempt  to  cross  the  box  sets  the  flag 
which  indicates  that  clipping  occurs.  Collision  recognition  discussed  in 
section  5.2.  is  machine  independent  and  is  able  to  cope  with  any  number 
of  obstacles.  Collision  recognition  is  carried  out  by  testing 
interferences  of  robot  link  with  obstacles  in  which  robot  links  are 
represented  by  line  segments. 
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Coll is ion- free  paths  are  obtained  through  the  geometric  mapping 
discussed  in  Chapters  3 and  4 with  the  assumption  that  a point  moves  along 
the  paths.  Since  a robot  end-effector  cannot  be  assumed  to  be  a point, 
modifications  to  obstacles  have  to  be  made  so  that  the  final  path  can 
maintain  enough  clearance  from  obstacles.  One  approach  to  this  problem 
is  to  expand  obstacles  by  an  amount  which  is  greater  than  one -half  of  the 
width  of  the  end-effector.  In  a related  work,  expansion  of  obstacles  is 
described  in  Lozano-Pdrez  and  Wesley  (1979)  as  grown  obstacles.  In  their 
work  obstacles  are  grown  depending  on  the  reference  point  and  orientation 
of  the  object  to  be  moved.  In  section  5.1,  expanding  two  and  three 
dimensional  obstacles  in  equal  amount  in  all  directions  is  considered. 

A collision-free  path  created  by  considering  the  expansion  of 
obstacles  still  has  limitations  in  guiding  robotic  manipulators.  Since 
the  base  joint  of  a robot  manipulator  is  attached  to  the  ground,  any  of 
the  other  links  may  hit  an  obstacle  or  other  links  while  the  end-effector 
moves  along  the  path  safely.  To  identify  such  a situation,  collisions 
have  to  be  tested  before  the  robot  moves  to  the  next  step.  Other  limiting 
cases  which  occur  are  when  the  path  is  out  of  the  reachable  workspace  or 
out  of  the  robot  actuator  range.  An  additional  algorithm  for  change  of 
configuration  is  introduced  to  overcome  these  situations.  When  a link  is 
about  to  hit  an  obstacle  or  a joint  angle  is  too  close  to  the  actuator 
limitation,  the  configuration  of  the  robot  is  changed  by  rotating  the  end- 
effector  orientation  to  alleviate  the  problem  while  maintaining  the  end- 
effector  position.  In  some  situation,  this  scheme  of  change  of 
configuration  may  not  solve  the  problem  without  altering  the  end-effector 
position.  A study  for  change  of  configuration  considering  both  altering 
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the  orientation  and  the  position  of  the  end-effector  is  not  attempted,  and 
left  for  future  research. 

A complete  solution  for  the  problems  associated  with  collision 
avoidance  can  be  achieved  by  obtaining  paths  in  joint  space.  Brooks 
(1983b).  He  demonstrated  such  collision-free  motion  for  a PUMA  robot 
where  the  first  three  joints  are  active.  In  his  work,  obstacles  in 
Euclidean  space  are  mapped  into  three  dimensional  joint  space  and  a 
collision-free  path  is  searched  through  connectivity  graphs. 

Similar  to  this,  a geometric  mapping  could  be  carried  out  in  joint 
space;  then  the  resulting  path  guarantees  a collision-free  motion  for  the 
robot.  However,  obtaining  the  joint  space  is  computationally  too 
expensive  to  use  in  real-time  control  of  a robot.  Control  of  a planar  3R 
robot  and  a General  Electric  P60  robot  are  considered  in  sections  5.4  and 
5.5,  respectively. 


5 . 1 Expansion  of  Obstacles 
5.1.1  Expansion  of  Two  Dimensional  Obstacles 

Fig.  5-1  shows  the  expansion  of  a two  dimensional  obstacle.  The 
original  obstacle  is  indicated  by  solid  lines,  and  an  expanded  and 
shrunken  obstacle  is  indicated  by  dashed  lines.  It  is  assumed  that  the 
numbering  of  vertices  is  counterclockwise  in  the  following  derivation. 
A proper  translation  of  edges  is  important.  For  instance,  L1 ' is  the 
translation  of  L1  to  the  right  and  L^"  is  translated  to  the  left.  The 
intersection  of  L1 ' and  L^'  produces  a vertex  P1 ' of  the  expanded  obstacle 
and  the  intersection  of  L,,"  and  L^"  produces  a vertex  P^'  of  the  shrunken 
obstacle.  The  line  L1  represents  the  edge  defined  by  vertices  P1  and  P2 
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Fig.  5-1  Expansion  of  an  obstacle 
and  is  expressed  in  parametric  form: 
r X = <P2x  - P1x>*  + P1x 

L1  : (5.1) 

y = (P2y  - V*  + P1y 

Translational  terms  are  necessary  to  express  lines  L1 ' and  L^'  by  adding 
these  terms  to  equation  (5.1).  For  L1 ' , the  translational  term  1»(1  » , 
ly')  is  given  by 

1.'  'I  <VP1y) 

(5.2) 

1y’  - - I <p2«-pix> 

where  1'  is  the  perpendicular  distance  between  L1  and  L1 ' and  L is  the 
distance  between  P1  and  P2.  For  L^’,  the  translational  term  l"(lx",l  ") 


is  given  by 
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V 


- 1<  vv 


(5.3) 


V = 


Addition  of 


V : 


T w • 


translational  terms  into  equation  (5.1)  yields 

' x = <P2X-P1x)t  + P1x  + V 

^ y = <P2y-P1y)t  + P1y  + V 

r x - ^x^lx^  + P1x  + lx" 


(5.4) 


(5.5) 


y = ‘W*  + P1y  + V' 

If  vertices  are  arranged  clockwise,  the  signs  of  translational  terms 
should  be  made  opposite  to  obtain  properly  expanded  or  shrunken  obstacles. 
No  assumptions  are  made  on  the  shapes  of  obstacles  hence  the  derivation 
works  for  either  convex  or  concave  obstacles. 

5.1.2  Expansion  of  Three  Dimensional  Obstacles 

Expansion  of  three  dimensional  obstacles  is  similar  to  that  of  two 
dimensional  cases.  Fig.  5-2  shows  expansion  of  a three  dimensional 
obstacle  where  the  expansion  amount  is  given  by  length  1.  Vector  a is 
perpendicular  to  the  surface,  S1 , which  is  defined  by  vertices  P1 , P2,  P3 
and  P^  and  is  expressed  by 


a = (P2  - P,)  x (P3  - £,) 

= (ax,  ay,  az)  . (5.6) 

The  magnitude  of  vector  a is  denoted  by  |a|  . Surface  S1  ’ , defined  by 
vertices  P1 ' , P2'  and  P3'  can  be  expressed  in  implicit  form: 

S/  = + C,z  - D,  = 0 (5.7) 


1 1 

1 

1 

1 1 

II 

p * p > 

1y  r2y 

p > p » 

r1z  r2z 

r3y 
P ’ 
r3z 

CO 

—A 

II 

P1z' 

P1x' 

p ' p ' 
r2z  r3z 

p > p ’ 

r2x  *3x 

where 


102 


Fig.  5-2  Expansion  of  a three-dimensional  obstacle 


C1  = 


1 

1 1 

P 9 
r1x 

p 9 
r2x 

P 9 
r3x 

p ’ 
r1x 
p ' 

*1y 

p • p * 

r2x  r3x 

p t p > 

*2y  *3y 

D1  = 

p 9 

iy 

p 9 
r1z 

V 

P2z' 

P 9 
3y 
P 9 
r3z 

The  vertices  for  S1 ' and  S,,"  are  obtained  by  translating  the 
vertices  of  S1 . The  translational  terms  for  are  given  by 


*x  = 


k| 


Ly  = 


U| 


(5.8) 


= 


I — I 


The  vertices  of  the  surface  S,',  P1 ' , P2',  P3'  and  P^' , are  obtained  by 
adding  translational  terms  to  vertices  P1 , P2,  P3  and  P^.  Likewise  the 
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vertices  of  the  surface  S^',  P,,",  P2",  P3"  and  P4",  are  obtained  by 

subtracting  translational  terms  from  P1 , P2,  P3  and  ^ respectively. 

A vertex  of  expanded  obstacle  V1  is  a common  intersection  of  three 
surface  S1 ' , S2'  and  S3'.  The  surfaces  S2'  is  defined  by  P, ' , P2'  and  P5', 

and  the  surface  S3'  is  defined  by  P, ' , P4'  and  P5'. 

S2'  = A2x  + B2y  + C2z  - D2  = 0 (5.9) 

S3'  = A3x  + B3y  + C3z  - D3  - 0 (5.10) 

Coordinates  of  V^(V^X,  V1y,  V1z)  can  be  obtained  by  solving  simultaneous 
equations  (5.7),  (5.9)  and  (5.10).  A proper  substitution  of  vertices  in 
the  above  formulae  is  essential  because  the  numbering  of  the  vertices  of 
a surface  is  random,  unlike  two  dimensional  cases. 

5 . 2 Collision  Recognitions 
5.2.1  Two  Dimensional  Collision  Recognition 

Shown  in  Fig.  5-3  is  a collision  between  a link  and  an  obstacle. 
Dashed  line  indicate  the  expansion  of  the  original  obstacle  where  the 
expansion  amount  is  one-half  of  the  width  of  the  robot  links,  so  links  can 
then  be  represented  by  straight  line-segments.  Links  and  edges  of 
obstacles  are  expressed  in  parametric  equations.  Intersections  between 
these  equations  determine  collisions.  Derivations  for  determining 
collision  between  link  and  an  edge  is  given  as  an  illustration. 

The  points  P1  and  P2  are  the  end  points  of  the  link  Lj.  The  link  1^  is 
represented  by  equation  G(u)  and  the  edge  ls  given  by  H(v)  : 

- x(u)  = (P2x-P1x)u  + P1x 
^ yOO  = (P2y-P1y)u  + P1y 


G(u) 


(5.11) 
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V3  V2 

l ~T 


E? 


H(v) 


r x(v)  = (v2x-v1x)v  + v 


'lx 


y(v)  - <v2y-v1y)v  + v1y 


(5.12) 


At  intersection,  G(u)  = H(v)  and  the  solutions  for  u and  v are  as  follows: 


<Vly-p1y)<P!x-F,x)  ‘ 

<V1x-pix><VV 

(5.13) 

U_  <V2x-V1xXW  - 

<VV<P2x-p,x> 

<VV(P1X-V1X>  - 

<V2x-V1x><W 

(5.14) 

<V2x-V1x>(W  - 

<VV<P2x-P1x> 

When  the  solution  for  v of  equation  (5.14)  is  in  the  range  0<v<l, 
the  edge  of  the  obstacle  does  intersect  the  line  passing  through  P1  and 
P2,  however  it  does  not  necessarily  intersect  with  the  line  segment  P1Z2. 
In  addition,  if  the  solution  for  u of  equation  (5.13)  is  in  the  range 
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0— ^l.  the  edge  of  the  obstacle  intersects  the  line  segment  representing 
link  Lj.  If  the  denominator  of  (5.14)  vanishes,  edge  and  link  L2  are 
parallel  or  collinear;  such  special  cases  can  be  considered  as  non- 
intersecting. The  calculation  has  to  be  carried  out  for  all  edges  of  the 
obstacles  to  determine  the  intersections  between  the  obstacles  and  robot 
links . 

5.2.2  Three  Dimensional  Collision  Recognition 

Collision  between  a link  of  a robot  and  obstacles  can  be  identified 
in  a way  similar  to  the  planar  case.  The  difference  is  that  intersections 
are  between  surfaces  of  obstacles  and  the  line  representing  links.  In 
Fig.  5-4,  the  obstacle  is  expanded  by  one-half  of  the  width  of  a robot 
link.  The  surface  A(s,t)  defined  by  vertices  Vq,  V1 , V2  and  V3  can  be 
expressed  in  a bilinear  parametric  equation: 


A(s,t) 


[(1-s) 


' 

" 

*0  v3 

(1-t) 

CO 

>1 

>T 

1 

t 

(5.15) 


where  the  parameter  range  of  s and  t are  0<s<l  and  0<t<l.  The  line  L(u) 
representing  a robot  link  is  given  by 

L(u)  = (P3  - P2)  u + P2  (5.16) 

where  the  valid  interval  of  parameter  u is  0<u<l  for  the  line  segment 
P2P3.  The  condition  for  finding  the  intersection  point  of  the  surface 
A(s , t)  and  the  line  L(u)  is 

A(s,t)  = L(u)  . (5.17) 

After  substituting  equations  (5.15)  and  (5.16)  into  (5.17)  and  rearranging 
the  equation  yields: 

A st  + B s + C t + Du  + Ev  = 0 

A ^ /\  J\  X 
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L(u) 

\ 


Fig.  5-4  Collision  between  a link  and  an  expanded  obstacle 
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z’  Dy’ 
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(5.18) 


substituting  the  y and  z components  of  P2,  P3,  Vq,  V1 , V2  and  V3. 
Eliminating  u from  equation  (5.18)  results  in  equations  in  s and  t: 


axst  + bxs  + cxt  + dx  = 0 


ayst  + bys  + Cyt  + dy 


0 


(5.19) 


107 


where 


ax 

- AxDz 

- AzDx 

ay  = Vz 

- AzDy 

bv 

= BD 

- B D 

b = B D 

- B D 

X 

X z 

Z X 

y y z 

z y 

cx 

- CXDZ 

- CzDx 

cy  ' CA 

- CzDy 

dx 

= ExDz 

- EzDx 

d,  - EyD; 

- EzDy 

Elimination  of  t leads  to  a quadratic  equation  in  s: 

<ayVaxVs2  + (bxVcxby+aydx’axdy>s  + (cydx'Cxdy)  = 0 • <5-20> 

Since  there  is  only  one  intersection  between  a line  and  a plane  surface, 

the  discriminant  of  (5.20)  will  vanish.  The  double  roots  of  s are  given 
by: 


s = 


be  - cb  + ad  - ad 
x y x y y x x y 

2 (a  b - a b ) 
x y y x' 


(5.21) 


and  a corresponding  solution  for  t can  be  obtained  from  one  of  the 


equations  of  (5.19): 


t = - 


V + dy 

V + cy 


(5.22) 


The  solution  for  u can  be  obtained  by  substituting  the  solutions  of  s and 
t into  one  of  the  equations  of  (5.18)  provided  that  the  denominator  does 
not  vanish.  If  the  solutions  for  s,  t and  u are  in  the  ranges  0<s<l, 
0<t<l  and  0<u<l  an  intersection  occurs,  since  0<s<l  and  0<t<l  represents 
the  surface  patch  defined  by  vertices  Vq,  V1 , V2  and  V3  of  the  obstacle 
and  0<u<l  represents  line  segment  ST. 

If  the  denominator  of  equation  (5.21)  or  (5.22)  vanishes,  the 
surface  and  the  line  are  parallel  or  coplanar  and  such  a case  can  be 
considered  as  non- intersecting . The  calculation  has  to  be  carried  out  for 
all  sides  of  the  obstacles  and  links  to  determine  collision.  One  should 
see  that  the  derivations  in  this  and  the  previous  sections  are 
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significantly  simpler  using  the  parametric  form  of  equations  than  using 
explicit  or  implicit  forms  of  equations. 

5 . 3 Change  of  Configurations 

As  described  at  the  introduction  of  this  chapter,  change  of 
configuration  is  used  to  avoid  collision  with  obstacles  and  to  stay  within 
the  range  of  joint  actuators.  An  assumption  is  made  that  the  end-effector 
orientation  may  change  along  the  path.  Whenever  a robot  is  about  to  hit 
any  obstacles  or  reach  joint  actuator  limits,  the  orientation  is  changed 
in  such  a way  that  the  robot  changes  configuration  to  alleviate  the 
problem  while  maintaining  the  original  end-effector  position.  One  should 
note  that  the  change  of  configuration  assumes  that  the  orientation  is  a 
free  parameter. 

5.3.1  Change  of  Orientation  of  Two  Dimensional  Case 

Supposing  that  a vector  r indicates  orientation  of  the  end-effector  and 
a matrix  M indicates  rotation  of  the  orientation.  Then  the  orientation 
vector  r'  after  the  rotation  is  given  by 

r'  = Mr  . (5.23) 

M may  be  expressed  as  follows: 


M = 


cos#  -sin# 
sin#  cos# 


(5.24) 


where  # is  the  rotation  angle  measured  counterclockwise. 

5.3.2  Change  of  Orientation  of  Three  Dimensional  Case 

Contrary  to  the  two  dimensional  case,  orientation  of  the  end- 
effector  for  the  three  dimensional  case  has  to  be  set  by  two  independent 
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Fig.  5-5  Rotation  of  orientation  vectors 
vectors.  The  vectors  u and  v in  global  coordinates  (the  X-Y-Z  coordinate 
system)  indicate  the  orientation.  Fig.  5-5.  Rotation  of  orientation 
should  be  taken  on  an  axis  that  provides  greater  dexterity.  Such  a 
rotation  axis  depends  on  the  robot  kinematic  structure  as  explained  in 
section  5.5.  The  axis  of  rotation  may  be  expressed  by  a unit  vector  a. 
A local  coordinate  system  (the  x-y-z  coordinates)  is  attached  to  the 
vector  a so  that  the  z-axis  is  aligned  with  a.  The  orientation  vectors 
u'  and  v'  after  rotation  about  the  a axis  can  be  expressed  as  follows: 


u'  = TMT'u 

(5.25) 

v ' = TMT'v 

(5.26) 

where  T is  an  orthogonal  transformation  matrix  and  M is  the  rotation 
matrix.  Matrix  T transforms  a vector  from  local  coordinates  to  global 
coordinates,  and  the  transpose  of  T,  T',  transforms  a vector  from  global 
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coordinates  to  local  coordinates.  Matrix  T may  be  obtained  from  the 
vector  a: 


<ax2+V>* 


axaz 


(ax2+ay2)» 


(ax2+a y»)’ 


a a 
y z 


(ax2+ay2)’ 


(ax2+ay2)%  az 


(5.27) 


The  rotation  is  taken  around  the  z-axis  of  the  local  coordinates,  hence 
the  rotation  matrix  is  given  by 


M = 


cos  # 
sin# 
0 


-sin# 

cos# 

0 


0 

0 

1 


(5.28) 


where  # is  the  amount  of  rotation  in  the  counterclockwise  direction. 
Details  of  two  and  three  dimensional  transformations  can  be  found  in 
Rogers  and  Adams  (1976). 


5 . 4 Planar  3R  Robot 

Shown  in  Fig.  5-6  is  a 3R  robot  work- environment  where  S and  T 
indicate  the  start  and  target  points.  The  large  circle  indicates  the 
workspace  boundary,  i.e.  the  boundary  of  the  extreme  reachable  workspace1. 


1 The  extreme  reachable  workspace  consists  of  all  points  that  are 
accessible  to  the  robot,  and  the  reachable  workspace  consist  of  all  points 

that  are  accessible  to  the  robot  for  a given  orientation,  the  definition 
appears  in  Kumar  and  Waldron  (1981). 


Ill 


Fig.  5-6  A 3R  robot  work- environment 

The  solid  lines  indicate  original  obstacles  and  dashed  lines  indicate 
expanded  obstacles.  The  amount  of  expansion  should  be  greater  than  one- 
half  the  width  of  the  end-effector.  The  circle  in  dashed  lines 
surrounding  the  base  joint  is  an  imaginary  obstacle  that  protects  the  base 
joint,  PQ.  In  some  cases  it  may  also  represent  the  inner  void  of  the 
workspace  where  the  robot  cannot  reach  if  the  base  link  is  longer  than  the 
sum  of  the  remaining  links . 
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Of  two  paths  shown  in  solid  and  dashed  lines,  the  path  in  dashed 
lines  cannot  be  used  because  the  robot  cannot  move  toward  the  target  point 
without  colliding  with  the  obstacle  between  the  path  and  the  base  joint. 
In  order  to  prevent  such  a path,  imaginary  barriers  connecting  obstacles 
and  the  workspace  boundary  are  added  which  are  shown  as  dashed  line 
segments.  In  the  process  of  geometry  mapping  the  expanded  obstacles,  the 
imaginary  barriers  and  the  imaginary  circle  representing  the  base  joint 
are  all  treated  as  obstacles.  Hence  the  path  by  a solid  line  can  be 
obtained  from  geometry  mapping.  One  may  note  that  the  paths  resulting 
from  the  geometric  mapping  are  always  bounded  by  the  limit  of  extreme 
reachable  workspace. 

As  explained  in  the  previous  section,  the  configuration  of  the 
robot  has  to  be  changed  in  three  different  occasions:  violation  of  the 
actuator  operating  limits,  movement  beyond  the  reachable  workspace  and 
collision  with  obstacles.  The  case  in  which  joint  actuator  limits  are 
exceeded  may  be  identified  by  comparing  the  result  of  reverse  analysis 
with  the  valid  ranges  of  the  actuators.  Movement  beyond  the  workspace  may 
be  identified  during  the  reverse  analysis,1  or  it  may  be  identified  by 
comparing  the  results  of  the  forward  analysis  and  the  input  data  to  the 
reverse  analysis.  Collision  with  obstacles  can  be  identified  by  the 
method  given  in  section  5.2. 

The  robot  is  guided  along  the  path  with  initially  given 
orientation.  Once  a limit  case  occurs  during  the  movement,  the  solutions 
of  the  reverse  analysis  are  discarded  and  the  input  data  for  the  previous 

1 A through  treatment  on  reverse  and  forward  analysis  of  robotic 
manipulators  can  be  found  in  Duffy  (1981). 
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Fig.  5-7  Change  of  orientation 

position  are  recalled.  The  motion  toward  the  target  point  stops  and  the 
change  of  configuration  is  carried  out.  The  allowable  range  of 
orientation  should  be  obtained  before  changing  the  orientation.  Shown  in 
Fig.  5-7  is  two  cases  of  change  of  orientation.  The  robot  changes 
orientation  at  point  a because  the  angle  of  joint  is  out  of  range,  and 
it  changes  orientation  at  point  b because  links  and  Lj  intersect  the 
obstacle . 

The  allowable  range  of  orientation  can  be  obtained  by  rotating  the 
orientation  in  both  directions  until  a limit  case  occurs,  so  that  the 
allowable  rotation  angles  are  obtained.  One  should  note  that  the 
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calculation  of  the  allowable  range  of  orientation  is  carried  out  in 
background  without  moving  the  robot.  After  obtaining  the  allowable  range 
of  orientation,  the  original  orientation  is  rotated  toward  the  mid- value 
of  the  allowable  range,  and  the  robot  then  moves  toward  the  target  point. 
The  calculation  of  allowable  range  of  orientation  is  required  only  if  the 
change  of  configuration  is  necessary.  After  the  change  of  configuration, 
the  robot  resumes  to  move  toward  target  point  with  the  new  orientation 
resulting  from  the  change  of  configuration. 

In  Young  and  Duffy  (1987),  the  method  of  computing  allowable  range 
of  rotation  of  a planar  3R  robot  is  via  transformation  of  two  line 
segments  representing  obstacle  edges  and  the  robot  link  to  a regular 
rectangle.  The  transformation  yields  an  expression  for  allowable  range 
of  rotation.  The  transformation  is  carried  out  for  each  link  and  the 
common  intersection  of  the  allowable  range  of  rotation  is  the  permissible 
range  of  rotation.  The  robot  configuration  is  then  determined  by  the  mid- 
value of  permissible  range  of  rotation.  This  process  is  repeated  each 
step  as  the  robot  moves  toward  target  point. 

An  animated  simulation  of  collision  avoidance  of  a 3R  robot  in 
conjunction  with  collision-free  path  generation  is  developed  on  a Silicon 
Graphics  IRIS  4D  graphics  workstation  with  the  UNIX  operating  system.  The 
C programming  language  and  the  graphics  library1  supported  by  the  system 
are  used  to  build  the  program.  The  hardware  z -buffering  saves 
considerable  time  in  managing  overlapped  images.  A uniform  shading  with 


1 Graphics  related  subjects  are  not  included  in  this  dissertation. 
One  may  find  excellent  treatments  of  the  subject  in  Rogers  (1985)  and 
Foley  and  Van  Dam  (1982). 
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one  light  source  at  infinity  is  employed  to  produce  realistic  images. 
Shown  in  Fig.  5-8  is  a three  dimensional  modeling  of  a 3R  robot,  obstacles 
and  a collision-free  path  for  given  start  and  target  points.  The  program 
includes  interactive  control  of  parameters  using  a mouse  and  function 
keys.  The  main  menu  includes  change  of  setting,  motions  with  and  without 
joint  limitations  and  program  termination.  The  submenus  of  the  change  of 
setting  include  the  change  of  start  position,  target  position,  obstacle 
location  and  path  parameters.  The  menu  items  included  in  the  change  of 
path  parameter  are:  shorter  path,  safer  path  and  increase  and  decrease 
parameter  t.  The  scene  can  be  rotated  and  scaled  with  the  mouse,  and 
translated  with  the  cursor  keys.  Pressing  the  function  key  FI  activates 
and  deactivates  the  drawing  of  expanded  obstacles  in  wireframe,  and  the 
F2  key  is  for  drawing  the  path.  The  escape  key  terminates  the  program  at 
any  time.  Shown  in  Figures  5-8  through  5-13  are  some  examples  of  the 
program  operation. 
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Fig.  5-8  Initial  settings  and  a collision-free  path. 


Fig.  5-9  A collision-free  path  after  changing  start  point 
target  point  and  location  of  obstacles 
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Fig.  5-10  Before  the  collision  with  an  obstacle 
(change  of  configuration  is  triggered) 


Fig.  5-11  After  the  change  of  configuration 
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Fig.  5-12  One  step  before  movement  beyond  the  reachable  workspace 
(change  of  configuration  is  triggered) 


Fig.  5-13  After  the  change  of  configuration 
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5 . 5 General  Electric  P60  Robot 

The  limit  cases  for  a GE  (General  Electric)  P60  robot  are  basically 
the  same  as  for  the  planar  3R  robot.  Shown  in  Fig.  5-14  is  a kinematic 
model  of  a GE  P60  robot;  the  displacement  analysis  is  given  in  Appendix 
B.  A computationally  efficient  method  for  determining  if  the  path  is  out 
of  the  reachable  workspace  is  to  use  the  displacement  analysis.  Violation 
of  the  valid  joint-actuator  range  is  determined  by  observing  the  results 
of  the  reverse  analysis.  Collision  between  a link  and  an  obstacle  was 
discussed  in  section  5.2.2  where  the  obstacle  was  expanded  and  the  robot 
links  were  represented  by  line-segments.  In  order  to  represent  a link  as 
a line-segment,  position  of  end  points  of  each  link  should  be  computed. 
Interference  of  robot  links  with  obstacles  can  be  monitored  by  computing 
intersections  between  the  line  segments  representing  links  and  surface 
patches  representing  obstacles.  It  is  also  possible  to  represent  the 
robot  link  as  polyhedra,  such  as  rectangular  prism,  and  then  intersections 
between  edges  of  links  and  obstacles  are  determined.  This  procedure, 
however,  will  take  more  computation  time. 

As  explained  in  section  5.3.2,  change  of  configuration  is  carried 
out  by  rotating  the  orientation  vectors  without  altering  the  end-effector 
position.  In  the  case  of  a planar  3R  robot,  the  axis  of  rotation  is 
defined  by  the  z-axis  where  the  robot  operates  in  the  x-y  plane.  In  the 
case  of  the  GE  P60  robot,  the  rotation  axis  should  be  selected  in  the 
direction  that  provides  greater  allowable  range  of  rotation;  such  an  axis 
depends  on  the  configuration  of  the  robot.  Instead  of  finding  the  axis 
that  provides  the  greatest  allowable  range  of  rotation,  several  axes  which 
are  parallel  to  S^,  S$  and  a^5  and  attached  to  the  end-effector  are 
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selected.  When  a limiting  case  occurs,  that  axis  which  provides  the 
greatest  allowable  range  of  orientation  is  determined  from  these 
preselected  axes.  The  allowable  range  of  rotation  can  be  calculated  by 
rotating  the  orientation  in  both  directions  until  a limit  case  occurs  and 
the  total  rotation  angle  is  the  allowable  range  of  orientation.  Rotation 
of  orientation  may  be  carried  out  by  substituting  a rotation  axis  vector, 
for  example  S5,  into  (5.27)  and  rotation  angle  into  (5.28),  then  the 
resulting  orientaion  vectors  can  be  obtained  from  (5.25)  and  (5.26).  It 
is  possible  that  after  the  change  of  orientation  for  the  given  set  of 
rotation  axes,  the  robot  still  may  not  be  able  to  avoid  the  obstacle, 
which  indicates  a failure.  Such  cases  may  require  altering  the  end- 
effector  position  to  avoid  obstacles. 


121 


The  animated  simulation  of  collision  avoidance  of  a GE  P60  robot 
in  conjunction  with  collision-free  path  generation  is  carried  out  on  a 
Silicon  Graphics  IRIS  4D/70  graphics  workstation.  Most  of  the  features 
are  similar  to  that  of  the  planar  case.  Additional  features- -toggled  by 
function  keys- - include  display  of  the  projection  plane,  the  path  in  three 
dimensions  and  the  windows  for  projected  images,  images  of  obstacle  in 
parameter  space  and  help  messages. 

Although  great  amount  of  time  is  devoted  to  develop  the  simulation 
program,  details  of  the  program  are  not  included  here  mainly  due  to  the 
size  of  program  which  is  over  220  pages.  Explanation  of  the  program  such 
as  organization  of  three  dimensional  object  data  structures,  manipulation 
of  fonts,  method  of  shading,  manipulation  of  windows  and  transformations 
of  graphic  objects  would  be  much  greater  than  the  size  of  the  program 
itself . 

Shown  in  Figures  5-15  through  5-21  are  some  examples  of  the 
operation  of  the  program.  Shown  in  Fig.  5-15  is  a GE  P60  robot,  a 
collision-free  path,  a projection  plane  shaded  in  transparency,  a window 
for  obstacle  images  on  the  projection  plane  and  another  window  for 
obstacle  images  in  parameter  space.  Shown  in  Figures  5-16  and  5-17  are 
before  and  after  the  change  of  configuration  due  to  imminent  collision. 
Shown  in  Figures  5-18  and  5-19  are  before  and  after  the  change  of 
configuration  because  the  second  joint  is  about  to  exceed  the  valid  range 
of  rotation.  A collision-free  path  after  changing  the  position  of  the 
start,  target  and  obstacles  is  shown  in  Fig. 5-20.  Shown  in  Fig.  5-21  is 
an  example  of  the  change  of  the  start,  target  and  an  obstacle. 
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Fig.  5-15  Initial  settings  with  a collision-free  path 


Fig.  5-16  One  step  before  the  collision, 
(change  of  configuration  is  triggered) 
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Fig.  5-17  After  change  of  configuration  due  to  collision 


Fig.  5-18  One  step  before  the  valid  range  of  a joint  actuator  is 
exceeded,  (change  of  configuration  is  triggered) 
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Fig.  5-19  After  the  change  of  configuration 


Fig.  5-20  A collision-free  path  after  changing  the  target  position 
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Fig.  5-21  Two  windows  are  opened  to  change  an  obstacle  position 

5 . 6 Concluding  Remarks 

In  this  chapter,  expansion  of  obstacles,  collision  recognition  and 
change  of  configuration  for  two  and  three  dimensional  cases  were  treated. 
Obstacles  were  expanded  an  equal  amount  in  all  directions  and  no 
assumptions  were  made  on  the  shape  of  obstacles.  Collision  recognition 
was  carried  out  by  intersections  between  links  and  obstacles.  In  deriving 
the  formulae  for  intersections,  parametric  forms  of  equations  were  used 
to  simplify  the  derivation.  The  collision  recognition  can  be  added  to 
existing  robot  control-loops  to  reduce  the  danger  of  collision  and  to 
restrict  the  motion  of  robots  in  a designated  space. 

A change  of  configuration  was  introduced  to  avoid  limit  cases  such 
as  exceeding  the  joint-actuator  ranges,  moving  out  of  the  reachable 
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workspace  and  collision  with  obstacles.  It  was  carried  out  by  rotating 
the  orientation  vectors  while  the  end-effector  stayed  at  the  same 
position.  In  the  case  of  a spatial  6R  robot,  several  rotation  axes  were 
preselected  and  one  of  the  axes  that  provides  the  greatest  allowable  range 
was  chosen. 

The  collision  avoidance  was  developed  in  conjunction  with 
collision-free  path  generation.  A collision-free  path  is  predefined  by 
geometric  mapping  and  the  robot  follows  the  path.  If  a limit  case  occurs, 
the  configuration  is  changed  to  alleviate  the  problem  without  altering  the 
end-effector  position.  As  mentioned  in  the  introduction  of  this  chapter, 
an  absolute  collision  avoidance  can  be  achieved  by  planning  collision-free 
path  in  joint  space.  Identification  of  limit  cases,  therefore,  will  not 
be  necessary. 

An  animated  simulation  of  a planar  3R  robot  and  a GE  P60  robot  were 
developed  on  a Silicon  Graphics  IRIS  4D  graphics  workstation.  The  three 
dimensional  solid  modeling  of  the  3R  and  the  GE  P60  robot  features  uniform 
shading  with  one  light  source.  The  start,  target  and  obstacle  position 
and  path  parameters  can  be  changed  interactively  with  the  mouse  and  the 
function  keys.  The  execution  time  for  obtaining  a collision-free  path 
depends  on  the  environment  settings.  Typical  examples  given  in  this 
chapter  take  less  than  one-hundred  milliseconds.  Comparisons  among 
different  algorithms  require  a benchmark  settings  such  as  standardized 
start,  target  points  and  obstacle  number  and  shape,  and  this  is  left  for 
future  work. 

The  simulation  reveals  that  the  execution  time  depends  on  the 
graphics  routine  rather  than  displacement  analysis  and  collision 
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recognition  of  the  robot.  Hence  a delay  at  the  graphics  subsystem  occurs 
as  more  objects  are  included  in  the  drawing  list  and  graphics  features 
such  as  opening  several  windows,  z-buffering,  shading  and  backfacing 
polygon  removal  are  added  in  the  graphics  routine. 


CHAPTER  6 
CONCLUSIONS 


6 . 1 Planning  Collision-Free  Paths 

Moving  a point  from  one  point  to  another  without  intersecting  any 
object  in  a workspace  is  the  fundamental  problem  of  path  planning 
research.  The  approach  taken  for  planning  collision-free  paths  is  via 
geometric  mapping  based  on  Bezier  curves  of  order  two.  Images  of 
obstacles  in  parameter  space  are  made  by  geometric  mapping,  and  a point 
selected  from  the  clear  area  corresponds  to  a collision-free  path.  When 
the  parameter  space  is  filled  with  obstacle  images,  intermediate 
connection  points  are  searched  and  a collision-free  path  for  each 
subinterval  is  obtained  by  geometric  mapping.  This  results  in  a 
connection  of  several  collision-free  paths.  Smoothing  the  connection 
region  is  carried  out  to  ensure  the  slope  continuity  throughout  the  path. 

The  algorithm  breaks  down  if  either  the  start  or  target  point  is 
inside  a maze,  or  if  it  is  enclosed  partially  (or  totally)  by  overlapping 
obstacles  or  concave  obstacles.  The  execution  time  for  geometric  mapping 
depends  on  how  the  algorithms  are  implemented,  the  computer  used  and  the 
environment  settings  such  as  the  number  and  shape  of  the  obstacles. 
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6 . 2 Collision  Avoidance  of  Robot  Manipulators 

Guiding  a robot  along  a given  path  while  avoiding  collisions  has 
been  performed.  The  path  is  obtained  by  the  previously  developed  method 
for  collision-free  paths.  Expansion  of  obstacles  and  addition  of 
imaginary  obstacles  are  introduced  to  exclude  the  paths  that  are 
unacceptable  to  guide  the  robot  from  the  process  of  geometric  mapping. 

Algorithms  for  collision  recognition  are  developed  to  identify 
intersections  between  robot  links  and  obstacles  and  between  robot  links 
themselves.  The  algorithm  can  be  implemented  in  real-time  collision 
recognition  to  complement  the  existing  robot  control-loops  and  to  reduce 
the  robot  programming  time. 

Change  of  configuration  is  developed  to  alleviate  limit  situations: 
collisions,  moving  out  of  the  reachable  workspace  and  exceeding  the  valid 
range  of  joint-actuators.  Configuration  is  changed  by  rotating  the  end- 
effector  orientation  while  staying  at  the  same  position. 

Animated  simulations  of  a planar  3R  robot  and  a GE  P60  robot  are 
carried  out  on  a Silicon  Graphics  IRIS  4D  graphics  workstation.  The 
simulations  show  that  the  overhead  due  to  collision  recognition,  when 
compared  with  the  same  simulation  without  collision  recognition,  is 
negligible  for  the  examples  given  in  Chapter  5. 

6 . 3 Further  Considerations 

As  mentioned  in  the  previous  chapters,  geometric  mapping  is  based 
on  Bezier  curves  of  order  two.  An  immediate  extension  of  the  geometric 
mapping  would  be  the  mapping  based  on  cubic  Bezier  curves.  It  may  require 
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more  computation,  however,  a search  for  intermediate  connection  points  is 
less  likely  to  be  required. 

One  of  the  assumptions  for  geometric  mapping  is  that  obstacles  are 
stationary.  In  the  case  of  moving  obstacles,  a new  geometric  mapping  may 
be  required  to  update  the  path  when  an  obstacle  moves  and  intersects  the 
established  path.  The  revision  of  the  path  may  depend  on  the  type  of 
robot.  For  a mobile  robot,  it  may  stay  in  a safe  area  until  the  path 
becomes  clear,  or  a new  path  from  the  current  point  to  the  target  point 
may  be  constructed  immediately.  This  area  requires  much  further 
investigation. 

Collision  avoidance  of  robot  manipulators  is  demonstrated  with  an 
assumption  that  the  path  to  follow  is  given  by  geometric  mapping. 
Geometric  mapping  here  transforms  obstacles  in  Euclidean  space  into 
parameter  space.  If  path  planning  is  carried  out  in  joint  space,  i.e. 
geometric  mapping  transforms  obstacle  Images  from  joint  space  to  parameter 
space,  manuevering  of  the  manipulator  to  avoid  limit  situations  will  not 
be  necessary.  Developing  computationally  efficient  methods  for  obtaining 
obstacle  images  in  joint  space  and  coping  with  the  resulting  high 
dimensionality  of  geometric  mapping  are  the  main  problems. 

Another  area  that  has  not  been  considered  is  the  comparison  of 
different  algorithms  for  planning  collision-free  paths.  This  may  require 
substantial  efforts  due  to  the  complexity  of  each  algorithm,  but  will 
provide  an  invaluable  insight  for  the  future  research  direction. 

Implementation  of  collision  recognition  and  path  planning  to  the 
existing  robot  control  loops  is  yet  another  area  where  industrial  robots 
may  benefit.  Data  for  obstacles  has  to  be  properly  entered  and  displayed 
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to  provide  correct  feedback  to  the  robot  operators.  This  will  require 
improved  I/O  handling  capabilities  of  current  robot  systems. 


APPENDIX  A 

SIMULATIONS  OF  PLANNING  COLLISION- FREE  PATHS 


Shown  in  the  Figures  A-l  through  A- 11  are  some  of  the  results  of 
simulations  executed  on  a VAX  11/750  mini-computer.  The  output  was 
produced  on  a Hewlett-Packard  7475A  plotter.  Expanded  obstacles  are 
plotted  in  dash-dot  lines.  Long  slender  rectangles  enclosing  other  small 
polygons  are  considered  to  be  walls  of  the  workspace.  Three  different 
environment  setting  are  given.  Shown  in  Figures  A-l  and  A- 2 are  random 
settings.  Figures  A-3  through  A-7  are  compartment  settings  and  Figures  A- 
8 through  A-ll  are  rooms  and  corridor  settings.  The  simulations  were 
carried  out  for  several  different  start  and  target  points. 
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Fig.  A-l  Random  setting  1 
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Fig.  A- 2 Random  setting  2 
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Fig.  A- 3 Compartment  setting  1 
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Fig.  A- 4 Compartment  setting  2 
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Fig.  A- 5 Compartment  setting  3 
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Fig.  A- 6 Compartment  setting  4 
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Fig.  A- 7 Compartment  setting  5 
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Fig.  A- 7 Rooms  and  corridor  setting  1 
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Fig.  A-9  Rooms  and  corridor  setting  2 
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Fig.  A- 10  Rooms  and  corridor  setting  3 
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Fig.  A-ll  Rooms  and  corridor  setting  4 


APPENDIX  B 

DISPLACEMENT  ANALYSIS  OF  THE  GE  P60  ROBOT 


The  displacement  analysis  of  the  GE  P60  robot,  which  consists  of 
a close -the -loop  analysis  and  the  reverse  and  forward  displacement 
analysis,  follows  the  method  established  by  Duffy  (1981).  Close -the -loop 
analysis  is  for  determining  the  unknown  parameters  of  the  hypothetical 
links  connecting  the  end-effector  and  the  ground  joint.  The  analysis  is 
due  to  Lipkin  and  Duffy  (1985),  and  is  not  repeated  here.  A GE  P60  robot 
has  parallel  structures  (a  four  bar  mechanism)  in  links  two  and  three. 
Analysis  of  these  four  bar  mechanisms  is  not  necessary  if  driving  the 
robot  is  the  only  concern,  however  it  is  necessary  for  displaying  the 
robot  on  a graphics  computer.  The  analysis  of  the  four  bar  mechanisms  is 
not  included  in  this  appendix. 

Shown  in  Fig.  B-l  is  a kinematic  model  of  the  GE  P60  robot.  The 
hypothetical  links  (S7,  a71)  are  drawn  in  dotted  lines.  Shown  in  Table  B- 
1 are  the  parameters  associated  with  the  robot  with  the  tool  dependent 
parameters  treated  as  variables.  Shown  in  Table  B-2  is  the  valid  range 
of  the  joint  actuators  in  which  the  angles  are  referenced  according  to  the 
kinematic  model. 
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Fig.  B-l  Kinematic  diagram  of  a GE  P60  robot 


Table  B-l  Parameters  of  GE  P60  robot 
(unit  = inch) 


S11  = * 

a12  = 0 

“12  " 90° 

S22  = 0 

a23  = 70 

“23=  °° 

s33  = 0 

a34  = 90 

“34=  °° 

S44=  98 

a45  = 0 

“45  = 90° 

S55  = 14.5 

a56  = 0 

“56  = 90° 

8 66  = 15-4 

a67  = ** 

“67  = ** 

S77  = * 

a71  = * 

“71  = * 

* : determined  from  close-the-loop  analysis 

**  : tool  dependent  parameters . 
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Table  B-2  The  valid  range  of  joint  actuators 


Joint  1 (ground  joint) 

-150°  < ^ < 150° 

Joint  2 (shoulder  joint) 

44°  < 8Z  < 130° 

Joint  3 (elbow  joint) 

-132°  < 8Z  < -70° 

Joint  4 

O 

O 

o 

CM 

VI 

>* 

VI 

o 

o 

CM 

1 

Joint  5 

-180°  < e5  < 180° 

Joint  6 

-300°  < e6<  300° 

B . 1 Reverse  Displacement  Analysis  of  GE  P60  Robot 
The  reverse  displacement  analysis  is  a process  for  obtaining  joint 
angles  , 02  through  9 6)  for  given  end-effector  position  (R)  and 

orientation  (a^,  S 7)  . The  angle  <f> 1 is  the  rotation  of  the  ground  joint, 
which  measures  from  the  x-axis  of  the  fixed  coordinates  (world 
coordinates)  to  the  x-axis  of  the  local  coordinates  of  the  ground  joint, 
Fig.  B-l . The  parameters  associated  with  the  hypothetical  joint  can  be 
calculated  via  a close-the-loop  analysis. 

The  vector  loop  equation  of  the  GE  P60  robot  is  given  by: 

S11^1  ‘ S44^4  + S55-5  + S66^6  + S77-7  + a23-23  + S34-34  + a67^67  + a71— 71  = 0 * 

(B.l) 

A scalar  product  of  equation  (B.l)  with  the  vector  S2  yields 

"S44  + S 66^6^2  + S77-7-2  + & 67^67-2  + a71— 71— 2 = 0 (B-2) 

Applying  set  14  of  the  direction  cosines  of  the  spatial  heptagon  from 

Duffy  (1981)  to  each  term  in  equation  (B.2)  yields: 

^2  = S67S7S1  ‘ (S71C67  + C71S67C7^C1 
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—71—2  = S1 

^67-2  = S1C7  + C1S7C71 

Substituting  these  results  into  equation  (B.2)  produces 


-S^  + Ac^  + Bs^  = 0 (B . 3) 

where  A = a67S7C71  - S66(s7ic67  + c71s67c7^  " S77s71 
B = a67c7  + S^s^sj  + a71 

Two  values  of  angle  6^  can  be  obtained  from  equation  (B.3)  using  the  half- 
tangent  angle  law. 


^ = 2tan  ( 


B ± (A2  + B2  - S^y 


A + S 


(B . 4) 


44 


Set  three  of  the  subsidiary  cosine  formula  yields  the  following 


relationship: 


ha  ~ h\  • C»-5> 

Expanding  the  term  in  each  side  produces  two  values  of  8 5: 

^5  = icos  (c^(s7^c^7  + c7^s^7c7)  - s^7s7s^)  . (B.6) 

The  vectors  S2,  S3  and  S^  are  parallel,  therefore,  the  subsidiary  formulae 
for  the  spherical  pentagon  can  be  used.  It  follows  that: 

X17  = ^6 
Yc 


l56  ^ X1S7  + Y1C7^ 


"56 


(B . 7) 
(B.8) 
(B.9) 


where 


*56  = S5C6 

X17  = S1C7  + C71S7 

Y56  = C67S5S6  + S67C5 
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J56 


S67S5S6  ‘ C67C5 


~s71c1 


Substitution  of  these  terms  into  equations  (B.7)  through  (B.9)  and 
elimination  of  c5  from  equations  (B.8)  and  (B.9)  yields: 


S1C7  + C71C1S7 


(B . 10) 


C67^C71C1C7  " sis7)  " S*7S71C 


1 7' 


67  71  1 


(B.ll) 


A unique  value  of  d6  can  be  obtained  from  equations  (B.10)  and  (B.ll). 
Substituting  x-  and  y-components  of  set  14  of  the  direction  cosines  for 
the  spatial  heptagon  into  equation  (B.l)  yields: 

-S44X5671  + S55X671  + S66X71  + S77X1  + a23C2  + a34W45671  + 


a67W71  + a71C1  = 0 

— S 1 1 s 1 2 - S44Y5671  + S55Y671  + S^Y^  + S/7Y1  - a23s2  - 

a34U456712  " a67U712  + a71U12  = 0 


(B . 12) 


(B . 13) 


where 


X567'l  = 0 

X671  = S6^C7C1  ' C71S7Sl)  + C67C6^S7C1  + C71C7S1  ^ “ S67C6S71S1 


II 

S67S7C1  + 

(s71c67  + C71  Si 

X1  = 

S71  s 1 

W45671 

= c2+3 

W71  = 

C1C7  ‘ S1 

S7C71 

Y5671 

= 0 

Y671  ' 

= -s6s71s7 

+ C67C6S71C7  + 

II 

S71S67C7  ‘ 

C71C67 

Y1  = 

"C71 
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U456712  “ S2+3 


U712  = S7S71 


U12  = 0 


One  may  see  that  the  unknown  terms  are  c2,  s2,  c2+3  and  s2+3.  The  equations 
(B . 12)  and  (B.13)  can  be  rearranged  in  the  following  form: 

(B . 14) 

(B . 15) 

where 


a23c2  + a34c2+3  A 


a23S2  + a34S2+3  “ B 


A “ S55X671  + S66X71  + S77X1  + a67W71  + a71C1 
B = -Sn  + S55Y671  + S^Y^  + S77Y1  - a67U712* 

Summation  of  the  squares  of  equations  (B.14)  and  (B.15)  yields: 

a232  + a342  + 2a23a34C3  = A2  + B2 
Two  values  of  6 3 are  obtained: 

A2  + B2  - a,,2  - a,,2 

6,  = icos"1  ( H * ) 

2 a23a34 


(B • 16) 


(B . 17) 


Substitution  of  03  into  equations  (B.14)  and  (B.15)  leads  to 

(a23  + a34c3)c2  - a34s3s2  = -A  (B.18) 

a34s3c2  (a23  a34c3^ s2  ~ B " (B • 19) 

The  unknowns  c2  and  s2  can  be  obtained  from  the  above  equations  and  the 
unique  value  of  angle  02  results.  Using  the  half -tangent  angle  law,  the 
remaining  unknown  angle  04  can  be  obtained: 


02+03+04  X65  - X1 

tan(  ) = 

2 Y65  - Y1 


(B . 20) 


After  expanding  the  right  side,  a unique  value  of  04  can  be  obtained: 
6.  = 2tan'1(  S67S6C5  + c67s5  ~ s71s1  ) . g 0 (B.21) 

H C C + C _ C.  J 

s67c6  + C71 
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When  the  solutions  are  properly  expanded,  eight  sets  of  solutions  result. 
Two  solution  sets  are  meaningful  due  to  the  limitations  on  the  range  of 
joint  actuators. 

B.2  Forward  Displacement  Analysis  of  GE  P60  Robot 
Forward  displacement  analysis  of  a robot  is  used  to  obtain  the 
position  and  orientation  of  the  end-effector  for  a given  set  of  joint 
angles.  In  order  to  obtain  proper  results  set  one  of  the  direction  cosine 
of  the  spatial  heptagon  should  be  used: 

r1  = *21*21  + a34— 34^  - S44^41  + S55^  + S6^6  + a 67 *6T  • (B.22) 

Expansion  of  the  components  are  as  follows: 

Rx1=a23C2+a34C2+3+S55S2+3+4+S66S5C2+3+4+a67^S6S2+3+4+C5C6C2+3+4^ 

V”  S44  + S66C5  ' a67S5C6 

Rz1=a23S2+a34S2+3'S55C2+3+4+S66S5S2+3+4+a67^C5C6S2+3+4'S6C2+3+4^ 

The  vector  R1  is  in  local  coordinates  defined  by  set  one,  therefore,  it 

should  be  transformed  to  world  coordinates: 

Rworld  = MR1  (B . 23) 

where  M is  the  rotation  matrix  given  by: 


M = 


cos<^1 

sin^1 

0 


-sin 0 
cos  <f>^  0 

0 1 


(B . 24) 


The  orientation  vectors  a^1  (a67x1  ’ a67y1  ’ a67z1  ) and  Sy1  (S^1 , S^1 , S7z1 ) are 
expressed  as  follows: 

a67x1  = s6s2+3+4  + C5C6S2+3+4 
a67y1  = ” S5C6 

*b7i  = ” S6C2+3+4  + C5C6S2+3+4 
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S7x  “ s67s6C5C2+3+4  + c67s5c2+3+4  " s67c6c2+3+4 


S7y1  " 'S67S6S5  + C67C5 


S7z  ~ S67S6C5S2+3+4  + C67S5S2+3+4  + S67C6S2+3+4 
The  orientation  vectors  in  world  coordinates  can  be  obtained  as  follows: 


a world  = Ma  1 
=67  n=67 

S7world  = MS/ 


(B . 25) 
(B.26) 


The  forward  displacement  analysis  is  useful  when  examining  if  the  data  to 
the  reverse  displacement  analysis  is  valid.  The  input  position  and 
orientation  of  the  end-effector  is  compared  with  the  results  of  the 
forward  displacement  analysis.  If  they  do  not  match,  the  out - of- the - 
reachable-workspace  situation  occurs. 
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